void itg3200Init(TWI_Master_t *title, int rate){
uint8_t gyrosetupbuffer1[4] = {0x15, (1000/rate) - 1, 0b00011000, 0x11};
uint8_t gyrosetupbuffer2[] = {0x3E, 0b00000001};
while(title->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(title, GYRO, gyrosetupbuffer1, 4, 0);
while(title->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(title, GYRO, gyrosetupbuffer2, 2, 0);
while(title->status != TWIM_STATUS_READY);
}
void adxl345Init(TWI_Master_t *title){
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001000, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
while(title->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(title, ACCEL, accelsetupbuffer1, 3, 0);
while(title->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(title, ACCEL, accelsetupbuffer2, 2, 0);
while(title->status != TWIM_STATUS_READY);
}
int main( void )
{
uint8_t RollSetupBuffer[2];
RollSetupBuffer[0] = 0x2D;
RollSetupBuffer[1] = 0x08;
uint8_t RollRead[2] = {0x32, 0x30};
uint8_t RollReadBuffer[2];
int convertcount;
PORTA.DIR = 0xff;
PORTD.DIR = 0xff;
PORTB.DIR = 0xff;
PORTF.DIR = 0x01;
//IMU Power
PORTC.DIR = (1 << 2) | (1 << 3);
PORTC.OUT = (1 << 3);
//Pullup Resistors
PORTCFG.MPCMASK = 0x03;
PORTC.PIN0CTRL = (PORTC.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc;
/* Enable LO interrupt level. */
PMIC.CTRL |= PMIC_LOLVLEN_bm;
sei();
TWI_MasterInit(&twiMaster, &TWIC, TWI_MASTER_INTLVL_LO_gc, TWI_BAUDSETTING);
while(twiMaster.status != TWIM_STATUS_READY){}
TWI_MasterWriteRead(&twiMaster, Roll, RollSetupBuffer, 2, 0);
while(twiMaster.status != TWIM_STATUS_READY){}
while(1)
{
TWI_MasterWriteRead(&twiMaster, Roll, &RollRead[1], 1, 8);
_delay_ms(100);
PORTF.OUT ^= 0x01;
while(twiMaster.status != TWIM_STATUS_READY){}
if((twiMaster.readData[0] & 0x80) == 0x80)
{
PORTA.OUT = twiMaster.readData[2];
}
}
return 0;
}
void lsm303dlhInit(TWI_Master_t *title){
uint8_t magsetupbuffer1[3] = {0, 0b00011000, 0b11100000};
uint8_t magsetupbuffer2[3] = {2, 0b00000000};
//uint8_t accelsetupbuffer1[3] = {0x20, 0b00110111};
while(title->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(title, MAG, magsetupbuffer1, 3, 0);
while(title->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(title, MAG, magsetupbuffer2, 2, 0);
while(title->status != TWIM_STATUS_READY);
//TWI_MasterWriteRead(title, ACCEL, accelsetupbuffer1, 2, 0);
//while(title->status != TWIM_STATUS_READY);
}
void gyro_read_gyro()
{
uint8_t data_buf[1];
data_buf[0] = STATUS_REG|128;
if((twiMaster.readData[0] & 0b00000100) && (twiMaster.status == TWIM_STATUS_READY)) // New data on z axis
{
gyro[0] = ((twiMaster.readData[2]<<8)|twiMaster.readData[1]) - gyro_zero[0];
gyro[1] = ((twiMaster.readData[4]<<8)|twiMaster.readData[3]) - gyro_zero[1];
gyro[2] = ((twiMaster.readData[6]<<8)|twiMaster.readData[5]) - gyro_zero[2];
TWI_MasterWriteRead(&twiMaster,GYRO_ADDRESS,data_buf,1,7);
}
else
TWI_MasterWriteRead(&twiMaster,GYRO_ADDRESS,data_buf,1,7);
}
void getmag(int * magcache, TWI_Master_t *imu){
unsigned char magstartbyte = 0x03;
int magprev;
int i;
//do{
while(imu->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(imu, MAG, &magstartbyte, 1, 6);
while(imu->result == TWIM_RESULT_UNKNOWN);
//}while(!(imu->readData[6] & 0x01));
for(i = 0; i < 3; i ++){
magprev = magcache[i];
magcache[i] = ((imu->readData[2*i] << 8) | (imu->readData[2*i + 1]));
magcache[i] = magcache[i] >> 0;
//magcache[i] = (magcache[i] + magprev)/2;
//magcache[i] = imu->readData[i];
if(abs(magcache[i]) > 200){
magcache[i] = magprev;
}
}
magcache[0] += 30;
magcache[1] += 10;
magcache[1] = (magcache[1] * 10)/9;
}
uint8_t gyro_read_byte(uint8_t address)
{
uint8_t data_buf[1];
data_buf[0] = address;
// Send address to read from and read 1 byte of data
return TWI_MasterWriteRead(&twiMaster,ACCEL_ADDRESS,data_buf,1,1);
}
void get_rgb(int16_t *r, int16_t *g, int16_t *b)
{
#ifdef AUDIO_DROPLET
uint8_t write_sequence = 0xB4;
uint8_t result = TWI_MasterWriteRead(RGB_SENSE_ADDR, &write_sequence, 1, 8);
uint16_t* temp_values = (uint16_t*)(twi->readData);
if(result)
{
//*c=temp_values[0];
*r=(int16_t)temp_values[1];
*g=(int16_t)temp_values[2];
*b=(int16_t)temp_values[3];
}
else printf_P(PSTR("Read failed.\r\n"));
#else
int16_t rTemp,gTemp,bTemp;
rTemp = get_red_sensor();
gTemp = get_green_sensor();
bTemp = get_blue_sensor();
rTemp = rTemp - r_baseline;
gTemp = gTemp - g_baseline;
bTemp = bTemp - b_baseline;
//if(rTemp<0) rTemp=0;
//if(gTemp<0) gTemp=0;
//if(bTemp<0) bTemp=0;
if(r!=NULL) *r = rTemp;
if(g!=NULL) *g = gTemp;
if(b!=NULL) *b = bTemp;
#endif
}
/*! \brief TWI read transaction.
*
* This function is a TWI Maste wrapper for read-only transaction.
*
* \param twi The TWI_Master_t struct instance.
* \param address The slave address.
* \param bytesToRead The number of bytes to read.
*
* \retval true If transaction could be started.
* \retval false If transaction could not be started.
*/
bool TWI_MasterRead(TWI_Master_t *twi,
uint8_t address,
uint8_t bytesToRead)
{
bool twi_status = TWI_MasterWriteRead(twi, address, 0, 0, bytesToRead);
return twi_status;
}
/*! \brief TWI write transaction.
*
* This function is TWI Master wrapper for a write-only transaction.
*
* \param twi The TWI_Master_t struct instance.
* \param address Slave address.
* \param writeData Pointer to data to write.
* \param bytesToWrite Number of data bytes to write.
*
* \retval 1 If transaction could be started.
* \retval 0 If transaction could not be started.
*/
uint8_t TWI_MasterWrite(uint8_t address,
uint8_t *writeData,
uint8_t bytesToWrite)
{
uint8_t twi_status = TWI_MasterWriteRead(address, writeData, bytesToWrite, 0);
return twi_status;
}
uint16_t gyro_read_word(uint8_t address)
{
uint8_t data_buf[1];
data_buf[0] = address|128;
// Send address to read from and read 2 bytes of data
return TWI_MasterWriteRead(&twiMaster,ACCEL_ADDRESS,data_buf,1,2);
}
/*! \brief TWI write transaction.
*
* This function is TWI Master wrapper for a write-only transaction.
*
* \param twi The TWI_Master_t struct instance.
* \param address Slave address.
* \param writeData Pointer to data to write.
* \param bytesToWrite Number of data bytes to write.
*
* \retval true If transaction could be started.
* \retval false If transaction could not be started.
*/
bool TWI_MasterWrite(TWI_Master_t *twi,
uint8_t address,
uint8_t *writeData,
uint8_t bytesToWrite)
{
bool twi_status = TWI_MasterWriteRead(twi, address, writeData, bytesToWrite, 0);
return twi_status;
}
int main(void)
{
// configure led
PORTF.DIR |= (1<<0); // EVAL-USB boards
// PORTF.DIR |= (1<<2); // EVAL-01 boards
// PORTD.DIR |= (1<<0); // EVAL-04 boards
// this example configuration is for ds2782 fuel gauge on PORTC TWIC with
// power provided on PORTC:2,3
// turn on power to ds2782
PORTC.OUT |= (1<<3); // PORTC:3 hi (power), PORTC:2 lo (gnd)
PORTC.DIR |= (1<<2) | (1<<3);
// Enable internal pull-up on PC0, PC1.. Uncomment if you don't have external pullups
// PORTCFG.MPCMASK = 0x03; // Configure several PINxCTRL registers at the same time
// PORTC.PIN0CTRL = (PORTC.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc; //Enable pull-up to get a defined level on the switches
/* Initialize TWI master. */
TWI_MasterInit(&twiMaster,
&TWIC,
TWI_MASTER_INTLVL_LO_gc,
TWI_BAUDSETTING);
/* Enable LO interrupt level. */
PMIC.CTRL |= PMIC_LOLVLEN_bm;
sei();
// loop forever
// do 16-bit master i2c read of ds2782 register
while (1)
{
// toggle led
PORTF.OUT ^= (1<<0); // EVAL-USB boards
// PORTF.OUT |= (1<<2); // EVAL-01 boards
// PORTD.OUT |= (1<<0); // EVAL-04 boards
// modify this address for the register you wish to read specific to your i2c device
sendBuffer[0]=0x0C; // read from 16bit voltage register
TWI_MasterWriteRead(&twiMaster,
SLAVE_ADDRESS,
&sendBuffer[0],
1,
2);
// results of 16bit read from volt register will be loaded in sendBuffer[0-1]
while (twiMaster.status != TWIM_STATUS_READY); // wait for transaction to complete
_delay_ms(100);
}
}
static void readRegs (TWI_Master_t *twi, uint8_t addr, uint8_t len)
{
txBuffer[0] = addr;
TWI_MasterWriteRead(twi, MCP79410_RTC_ADDR, txBuffer, 1, len);
TWI_MasterWait(twi, 100);
uint8_t *ptr = regsBuffer+addr;
for (uint8_t i=0 ; i < len ; i ++) {
*ptr++ = twi->readData[i];
}
}
void regWrite(uint8_t slaveAddress, uint8_t reg, uint8_t val)
{
uint8_t sendBuffer[2] = {reg, val};
/* Wait until transaction is complete !!!!!! */
while (twiMaster.status != TWIM_STATUS_READY) {}
TWI_MasterWriteRead(&twiMaster, slaveAddress, sendBuffer, 2, 0);
/* Wait until transaction is complete !!!!!! */
while (twiMaster.status != TWIM_STATUS_READY) {}
}
uint8_t twiWriteReadWrapper(uint8_t addr, uint8_t* writeData, uint8_t bytesToWrite, uint8_t bytesToRead, char* callerDescr){
uint32_t startTime = getTime();
uint8_t result = 0;
uint8_t printed = 0;
while(!result){
if((printed = waitForTWIReady(startTime, callerDescr))){
result = TWI_MasterWriteRead(addr, writeData, bytesToWrite, bytesToRead);
}else{
return 0;
}
}
return result + printed - 1;
}
void regRead(uint8_t slaveAddress, uint8_t *reg, uint8_t *buf, uint8_t count)
{
//Writes which register to read from
TWI_MasterWriteRead(&twiMaster, slaveAddress, reg, 1, count);
/* Wait until transaction is complete. */
while (twiMaster.status != TWIM_STATUS_READY) {}
//Read in the received data
for (int i = 0; i < count; i++)
{
*(buf+i) = twiMaster.readData[i];
}
}
void gyro_zero_accel()
{
uint8_t data_buf[1];
data_buf[0] = STATUS_REG|128;
// Read acceleration words and status register
TWI_MasterWriteRead(&twiMaster,ACCEL_ADDRESS,data_buf,1,7);
while (twiMaster.status != TWIM_STATUS_READY); // wait for transmission completion
if(twiMaster.readData[0] & 8) // If we have new data, save new "zero" values
{
accel_zero[0] = ((twiMaster.readData[2]<<8)|twiMaster.readData[1]);
accel_zero[1] = ((twiMaster.readData[4]<<8)|twiMaster.readData[3]);
accel_zero[2] = ((twiMaster.readData[6]<<8)|twiMaster.readData[5]);
}
}
void getgyro(int *gyrocache, TWI_Master_t *imu, uint8_t *gyrostartbyte){
int i;
uint8_t buffer[6];
do{
while(imu->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(imu, GYRO, gyrostartbyte, 1, 10);
while(imu->result == TWIM_RESULT_UNKNOWN);
}while(!(imu->readData[0] & 0x01));
for(i = 0; i < 5; i ++){
buffer[i] = imu->readData[i + 3];
}
for(i = 0; i < 5; i += 2){
gyrocache[i/2] = ((buffer[i] << 8) | buffer[i + 1])>>5;
}
}
void gyro_zero_gyro()
{
uint8_t data_buf[1];
data_buf[0] = STATUS_REG|128;
do
{
TWI_MasterWriteRead(&twiMaster,GYRO_ADDRESS,data_buf,1,7);
while (twiMaster.status != TWIM_STATUS_READY); // wait for transmission completion
}
while (!(twiMaster.readData[0] & 0b00000100)); // Try to read new z axis data until we get an update and a new value
// Update new value
gyro_zero[0] = ((twiMaster.readData[2]<<8)|twiMaster.readData[1]);
gyro_zero[1] = ((twiMaster.readData[4]<<8)|twiMaster.readData[3]);
gyro_zero[2] = ((twiMaster.readData[6]<<8)|twiMaster.readData[5]);
}
/*#####################################################*/
bool _I2C_tx(struct Twi_s* param, unsigned char addr, unsigned char *buff_send, unsigned int bytes_send)
{
if(!param)
return false;
#if (USE_DRIVER_SEMAPHORE == true)
while(twi_semaphore[param->TwiNr]);
twi_semaphore[param->TwiNr] = true;
#endif
param->MasterSlaveAddr = addr << 1;
param->TxBuff = buff_send;
bool result = TWI_MasterWriteRead(param, bytes_send, 0);
#if (USE_DRIVER_SEMAPHORE == true)
twi_semaphore[param->TwiNr] = false;
#endif
if(result == HAL_OK)
return true;
else
return false;
}
void getaccel(int *accelcache, TWI_Master_t *imu, uint8_t *accelstartbyte){
int i;
do{
while(imu->status != TWIM_STATUS_READY);
TWI_MasterWriteRead(imu, ACCEL, accelstartbyte, 1, 10);
while(imu->result == TWIM_RESULT_UNKNOWN);
}while(!(imu->readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu->readData[i + 3] & 0x80){
accelcache[i/2] -= 256 * (~imu->readData[i + 3] + 1);
accelcache[i/2] -= (~imu->readData[i + 2] + 1);
}
else{
accelcache[i/2] += 256 * imu->readData[i + 3];
accelcache[i/2] += (imu->readData[i + 2]);
}
}
for(i = 0; i < 3; i ++){
accelcache[i] = (accelcache[i] * 2);
}
}
int main(void){
enum states{running, stopped} state = stopped;
/**Move cmd vars*/
short int rise = 0;
short int rotate = 0;
short int forward = 0;
short int tilt = 0;
short int motorr = 0;
short int motorl = 0;
short int servor = 0;
short int servol = 0;
int i;
char xbeebuffer[100];
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
//Pulse width modulation setup for servos, port D
TCD1.CTRLA = TC_CLKSEL_DIV1_gc;
TCD1.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm;
TCD1.PER = 40000;
TCC1.CTRLA = TC_CLKSEL_DIV1_gc;
TCC1.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm;
TCC1.PER = 40000;
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 40000;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
//Setup IMU
PORTD.DIR = 0x30;
PORTC.DIR = 0b00111100;
PORTC.OUT = 0b00001000;
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer1, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWIC.MASTER.CTRLB |= 0x0C;
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(readdata){
readdata = 0;
sendchar(&xbee, input);
if(input == 'r'){
state = running;
}
else if(input == 's'){
state = stopped;
sprintf(xbeebuffer, "rise %4d tilt %4d rot %4d for %4d \n\r", rise, tilt, rotate, forward);
sendstring(&xbee, xbeebuffer);
}
else if(input == 'u'){
rise += 25;
}
else if(input == 'd'){
rise -= 25;
}
else if(input == 'c'){
rotate += 10;
}
else if(input == 'x'){
rotate -= 10;
}
else if(input == 'a'){
tilt += 10;
}
else if(input == 'e'){
tilt -= 10;
}
else if(input == 't'){
forward += 10;
}
else if(input == 'b'){
forward -= 10;
}
}
switch(state){
case stopped:
TCD1.CCA = 2000;
TCC1.CCA = SERVOLINI;
TCD1.CCB = 2000;
TCC1.CCB = SERVORINI;
break;
case running:
if(TCC0.INTFLAGS & 0x01){
TCC0.INTFLAGS = 0x01;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
PORTF.OUT = 2;
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x01));
for(i = 0; i < 5; i += 2){
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
PORTF.OUT = 3;
while(imu.result == TWIM_RESULT_UNKNOWN);
PORTF.OUT = 1;
}while(!(imu.readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu.readData[i + 3] & 0x80){
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else{
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
PORTF.OUT = 0;
}
for(i = 0; i < 3; i ++){
accelcash[i] /= DAMPENACCEL;
rollcash[i] /= DAMPENROLL;
}
ValueFunk(accelcash[0],accelcash[1],accelcash[2],rollcash[0],rollcash[1],rollcash[2],&servol,&servor,&motorl,&motorr);
while(TCD1.CNT < 4000);
TCD1.CCA = motorl + rise - tilt;
TCD1.CCB = motorr + rise + tilt;
/*
while(TCC1.CNT < 4000);
TCC1.CCA = servol + rotate + forward;
TCC1.CCB = servor - rotate + forward;
*/
sprintf(xbeebuffer, " X%4d x%4d R%4d L%4d\n\r", rollcash[1], accelcash[0],motorr, motorl);
sendstring(&xbee, xbeebuffer);
for(i = 0; i < 3; i ++){
accelcash[i] *= INTEGRATEACCEL;
rollcash[i] *= INTEGRATEROLL;
}
break;
}
}
return 0;
}
int main (void)
{
/* Initialize TWI master. */
TWI_MasterInit(&twiMaster,&TWIF,TWI_MASTER_INTLVL_LO_gc,TWI_BAUDSETTING);
TWIF.SLAVE.CTRLA=0; //slave disabled
//board_init();
En_RC32M();
//Enable LowLevel & HighLevel Interrupts
PMIC_CTRL |= PMIC_HILVLEN_bm | PMIC_LOLVLEN_bm |PMIC_MEDLVLEN_bm;
PORT_init();
TimerD0_init();
TimerC0_init();
//USARTE0_init();
//ADCB_init();
LCDInit();
//wdt_enable();
// Globally enable interrupts
sei();
LED_Green_Time = 3000;
LED_Green_Speed = 500;
LED_Red_Time = 3000;
LED_Red_Speed = 100;
LED_White_Time = 1000;
LED_White_Speed = 200;
Buzzer_Time = 2000;
Buzzer_Speed = 150;
///////////////////////////////////////////////////////////////////////////////////////////////Begin NRF Initialize
NRF24L01_L_CE_LOW; //disable transceiver modes
SPI_Init();
_delay_us(10);
_delay_ms(100); //power on reset delay needs 100ms
NRF24L01_L_Clear_Interrupts();
NRF24L01_L_Flush_TX();
NRF24L01_L_Flush_RX();
NRF24L01_L_CE_LOW;
NRF24L01_L_Init_milad(_TX_MODE, _CH, _2Mbps, Address, _Address_Width, _Buffer_Size, RF_PWR_MAX);
NRF24L01_L_WriteReg(W_REGISTER | DYNPD,0x01);
NRF24L01_L_WriteReg(W_REGISTER | FEATURE,0x06);
NRF24L01_L_CE_HIGH;
_delay_us(130);
///////////////////////////////////////////////////////////////////////////////////////////////END NRF Initialize
// Insert application code here, after the board has been initialized.
while(1)
{
LCDGotoXY(0,0);
sprintf("salam");
//LCDStringRam(str);
////////TWI
//tx1[0]=Robot_D[RobotID].M1;
//tx2[0]=Robot_D[RobotID].M2;
//tx3[0]=Robot_D[RobotID].M3;
//tx4[0]=Robot_D[RobotID].M4;
tx1[0]=0x0F;
tx2[0]=0x12;
tx3[0]=0x01;
tx4[0]=0x02;
// TWI_MasterWriteRead(&twiMaster,SLAVE1_ADDRESS,&tx1[0],1,3);
// TWI_MasterWriteRead(&twiMaster,SLAVE2_ADDRESS,&tx2[0],1,3);
TWI_MasterWriteRead(&twiMaster,SLAVE3_ADDRESS,&tx3[0],1,3);
rx3[0]=twiMaster.readData[0];
if (rx3[0]==1)
{
LED_White(ON);
}
// TWI_MasterWriteRead(&twiMaster,SLAVE4_ADDRESS,&tx4[0],1,3);
/////////////////////
NRF24L01_L_Write_TX_Buf(Buf_Tx_L, _Buffer_Size);
NRF24L01_L_RF_TX();
if (Buf_Rx_L[0]=='f')
{
LED_Green_PORT.OUTTGL = LED_Green_PIN_bm;
}
_delay_ms(10);
}
}
/*! \brief TWI read transaction.
*
* This function is a TWI Maste wrapper for read-only transaction.
*
* \param twi The TWI_Master_t struct instance.
* \param address The slave address.
* \param bytesToRead The number of bytes to read.
*
* \retval 1 If transaction could be started.
* \retval 0 If transaction could not be started.
*/
static uint8_t TWI_MasterRead(uint8_t address, uint8_t bytesToRead){
uint8_t twi_status = TWI_MasterWriteRead(address, 0, 0, bytesToRead);
return twi_status;
}
/*! /brief Example code
*
* Example code that reads the key pressed and show a value from the buffer,
* sends the value to the slave and read back the processed value which will
* be inverted and displayed after key release.
*/
int main(void)
{
/* Initialize PORTE for output and PORTD for inverted input. */
PORTE.DIRSET = 0xFF;
PORTD.DIRCLR = 0xFF;
PORTCFG.MPCMASK = 0xFF;
PORTD.PIN0CTRL |= PORT_INVEN_bm;
// PORTCFG.MPCMASK = 0xFF;
// PORTD.PIN0CTRL = (PORTD.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc;
// Enable internal pull-up on PC0, PC1.. Uncomment if you don't have external pullups
// PORTCFG.MPCMASK = 0x03; // Configure several PINxCTRL registers at the same time
// PORTC.PIN0CTRL = (PORTC.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc; //Enable pull-up to get a defined level on the switches
/* Initialize TWI master. */
TWI_MasterInit(&twiMaster,
&TWIC,
TWI_MASTER_INTLVL_LO_gc,
TWI_BAUDSETTING);
/* Initialize TWI slave. */
TWI_SlaveInitializeDriver(&twiSlave, &TWIC, TWIC_SlaveProcessData);
TWI_SlaveInitializeModule(&twiSlave,
SLAVE_ADDRESS,
TWI_SLAVE_INTLVL_LO_gc);
/* Enable LO interrupt level. */
PMIC.CTRL |= PMIC_LOLVLEN_bm;
sei();
uint8_t BufPos = 0;
while (1) {
while(!PORTD.IN); /* Wait for user to press button */
switch(PORTD.IN){
case (PIN0_bm): BufPos = 0; break;
case (PIN1_bm): BufPos = 1; break;
case (PIN2_bm): BufPos = 2; break;
case (PIN3_bm): BufPos = 3; break;
case (PIN4_bm): BufPos = 4; break;
case (PIN5_bm): BufPos = 5; break;
case (PIN6_bm): BufPos = 6; break;
case (PIN7_bm): BufPos = 7; break;
default: break;
}
/* Show the byte to send while holding down the key. */
while(PORTD.IN != 0x00){
PORTE.OUT = sendBuffer[BufPos];
}
TWI_MasterWriteRead(&twiMaster,
SLAVE_ADDRESS,
&sendBuffer[BufPos],
1,
1);
while (twiMaster.status != TWIM_STATUS_READY) {
/* Wait until transaction is complete. */
}
/* Show the sent byte received and processed on LEDs. */
PORTE.OUT = (twiMaster.readData[0]);
while(PORTD.IN); /* Wait for user to release button */
}
}
int main(void){
enum states{running, stopped} state = stopped;
char input;
int i;
char xbeebuffer[100];
uint8_t accelsetupbuffer1[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelsetupbuffer2[3] = {0x31, 0x00};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
TCC0.CTRLB = TC_WGMODE_SS_gc;
TCC0.PER = 40000;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
//Setup IMU
PORTC.DIR = 0b00001100;
PORTC.OUT = 0b00001000;
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer1, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
/**Setup Xbee*/
PORTE.DIR = 0b00001000;
PORTF.DIR = 3;
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
while(1){
if(USART_RXBufferData_Available(&xbee)){
input = USART_RXBuffer_GetByte(&xbee);
sendchar(&xbee, input);
if(input == 'r'){
state = running;
}
else if(input == 's'){
PORTF.OUT ^= 0x02;
state = stopped;
}
}
switch(state){
case stopped:
break;
case running:
if(TCC0.INTFLAGS & 0x01){
for(i = 0; i < 3; i ++){
rollcash[i] = 0;
accelcash[i] = 0;
}
TCC0.INTFLAGS = 0x01;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x01));
for(i = 0; i < 5; i += 2){
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
PORTF.OUT = 1;
do{
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
while(imu.result == TWIM_RESULT_UNKNOWN);
}while(!(imu.readData[0] & 0x80));
for(i = 0; i < 5; i += 2){
if(imu.readData[i + 3] & 0x80){
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else{
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
sprintf(xbeebuffer, "%d %d\n\r", rollcash[0], accelcash[0]);
sendstring(&xbee, xbeebuffer);
}
break;
}
}
return 0;
}
/*#####################################################*/
bool _SetupI2CTransmit(new_twi* TwiStruct, unsigned int TransmitBytes)
{
return TWI_MasterWriteRead(TwiStruct, TransmitBytes, 0);
}
/*#####################################################*/
bool _SetupI2CReception(new_twi* TwiStruct, unsigned int TransmitBytes, unsigned int ReceiveBytes)
{
return TWI_MasterWriteRead(TwiStruct, TransmitBytes, ReceiveBytes);
}
int main(void) {
int translen = 0;
char xbeebuffer[100];
int i;
int bytetobuffer;
char rollread = 0;
char accelread = 0;
int dataready = 0;
char receive;
char count = 0;
uint8_t accelsetupbuffer[3] = {0x2C, 0b00001100, 0x08};
uint8_t accelstartbyte = 0x30;
uint8_t rollsetupbuffer1[4] = {0x15, 0x04, 0x19, 0x11};
uint8_t rollsetupbuffer2[] = {0x3E, 0b00000001};
uint8_t rollstartbyte = 0x1A;
char rollcash[3] = {0,0,0};
int accelcash[3] = {0,0,0};
short int motorr = 0;
short int motorl = 0;
short int servor = 0;
short int servol = 0;
enum states {running, stopped} state = stopped;
/**Setup directions for serial interfaces*/
PORTC.DIR = 0b00001000;
PORTC.OUT = 0b00001000;
PORTE.DIR = 0b00001000;
PORTF.DIR = 0x03;
PORTD.DIR = 0x0F;
//Pulse width modulation setup for servos, port D
TCD0.CTRLA = TC_CLKSEL_DIV1_gc;
TCD0.CTRLB = TC_WGMODE_SS_gc | TC0_CCAEN_bm |TC0_CCBEN_bm | TC0_CCCEN_bm | TC0_CCDEN_bm;
TCD0.PER = 40000;
/**Enable pullup resistors for imu*/
PORTCFG.MPCMASK = 0x03;
PORTC.PIN0CTRL = (PORTC.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc;
/**Setup interrupts*/
PMIC.CTRL |= PMIC_LOLVLEX_bm | PMIC_MEDLVLEX_bm | PMIC_HILVLEX_bm |
PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
sei();
/**Setup IMU*/
TWI_MasterInit(&imu, &TWIC, TWI_MASTER_INTLVL_HI_gc, TWI_BAUDSETTING);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ACCEL, accelsetupbuffer, 3, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer1, 4, 0);
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, rollsetupbuffer2, 2, 0);
while(imu.status != TWIM_STATUS_READY);
/**Setup Xbee*/
USART_InterruptDriver_Initialize(&xbee, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(xbee.usart, USART_CHSIZE_8BIT_gc, USART_PMODE_DISABLED_gc, false);
USART_RxdInterruptLevel_Set(xbee.usart, USART_RXCINTLVL_HI_gc);
USART_Baudrate_Set(&USARTE0, 12 , 0);
USART_Rx_Enable(xbee.usart);
USART_Tx_Enable(xbee.usart);
setup = 0;
readdata = 0;
while(1) {
if(USART_RXBufferData_Available(&xbee)) {
receive = USART_RXBuffer_GetByte(&xbee);
if(receive == 's') {
state = running;
}
else if(receive == 'n') {
state = stopped;
}
}
switch(state) {
case stopped:
PORTF.OUT = 3;
TCD0.CCA = 2000;
TCD0.CCC = 2000;
TCD0.CCB = 3000;
TCD0.CCD = 3000;
for(i = 0; i < 3; i ++) {
accelcash[i] = 0;
rollcash[i] = 0;
}
break;
case running:
PORTF.OUT = 0;
//Roll reading
while(imu.status != TWIM_STATUS_READY);
TWI_MasterWriteRead(&imu, ROLL, &rollstartbyte, 1, 10);
while(!readdata);
readdata = 0;
PORTF.OUT |= 0x01;
for(i = 0; i < 5; i += 2) {
if(imu.readData[i + 3] & 0x80) {
accelcash[i/2] -= 256 * (~imu.readData[i + 3] + 1);
accelcash[i/2] -= ~imu.readData[i + 2] + 1;
}
else {
accelcash[i/2] += 256 * imu.readData[i + 3];
accelcash[i/2] += imu.readData[i + 2];
}
}
//Accel reading
while(imu.status != TWIM_STATUS_READY);
PORTF.OUT ^= 1;
TWI_MasterWriteRead(&imu, ACCEL, &accelstartbyte, 1, 10);
while(!readdata);
readdata = 0;
for(i = 0; i < 5; i += 2) {
rollcash[i/2] += ((char)(imu.readData[i + 3]));
}
PORTF.OUT |= 0x02;
count ++;
if(count > 4) {
for(i = 0; i < 3; i ++) {
accelcash[i] /= 5;
rollcash[i] /= 2;
}
//motor updates
rollcash[0] -= RXN;
rollcash[1] -= RYN;
rollcash[2] -= RZN;
accelcash[0] -= AXN;
accelcash[1] -= AYN;
accelcash[2] -= AZN;
ValueFunk(accelcash[0],accelcash[1],accelcash[2],rollcash[0],rollcash[1],rollcash[2],&servol,&servor,&motorl,&motorr);
while(TCD0.CNT < 4000);
TCD0.CCA = motorr;
TCD0.CCB = servor;
TCD0.CCC = motorl;
TCD0.CCD = servol;
sprintf(xbeebuffer, " X%4d Y%4d Z%4d x%4d y%4d z%4d R%4d r%4d L%4d l%4d\n\r", rollcash[0], rollcash[1], rollcash[2], accelcash[0], accelcash[1], accelcash[2], motorr, servor, motorl, servol);
for(i = 0; xbeebuffer[i] != 0; i ++) {
bytetobuffer = 0;
while(!bytetobuffer) {
bytetobuffer = USART_TXBuffer_PutByte(&xbee, xbeebuffer[i]);
}
}
for(i = 0; i < 3; i ++) {
accelcash[i] = 0;
}
}
break;
}
}
return 0;
}
