void i2cInit(){
PORTB.DIRCLR = PIN5_bm;
PORTB.PIN5CTRL = PORT_OPC_WIREDOR_gc;
PORTE.DIRSET = PIN0_bm | PIN1_bm;
twi = &twiMaster;
TWI_MasterInit(&TWIE, TWI_MASTER_INTLVL_HI_gc, TWI_BAUD(F_CPU, 100000));
}
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);
}
}
void i2c_init()
{
if (i2c_initialized)
return;
// Initialize TWI master.
TWI_MasterInit(&i2c, &TWIE, TWI_MASTER_INTLVL_LO_gc, TWI_BAUD(F_CPU, 400000));
PMIC.CTRL |= PMIC_LOLVLEN_bm;
i2c_initialized = true;
}
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;
}
int main(void)
{
facilitatePowersaving(); // Pull-ups and PR Registers
initHardware(); // Switches, LEDs, Sleep, Interrupt
sei(); // Enable global interrupts
/* === Initialization of TWI Module === */
// Enable internal pull-up on PD0, PD1 for correct TWI operation
PORTCFG.MPCMASK = 0x03;
TWIPORT.PIN0CTRL = (TWIPORT.PIN0CTRL & ~PORT_OPC_gm) | PORT_OPC_PULLUP_gc;
/* Initialize TWI master. */
TWI_MasterInit(&twiMaster,
&TWI,
TWI_MASTER_INTLVL_LO_gc,
TWI_BAUDSETTING);
/* Initialize TWI slave. */
TWI_SlaveInitializeDriver(&twiSlave, &TWI, TWI_SlaveProcessData);
TWI_SlaveInitializeModule(&twiSlave,
OWN_ADDRESS,
TWI_SLAVE_INTLVL_LO_gc);
/* === End Initialization === */
while (1) {
// Try to sleep while we are waiting for button press
while (READ_SWITCHES == 0x00){ //See board.h for READ_SWITCHES define.
sleep();
}
_delay_ms(5); // Debounce switch
sendBuffer[0] = READ_SWITCHES;
TWI_MasterWrite(&twiMaster, // Module
OTHER_ADDRESS, // Which slave
&sendBuffer[0], // What to send
1); // Send how much
/* Wait until transaction is complete. Required TWI interrupts will be executed while waiting */
while (twiMaster.status != TWIM_STATUS_READY);
// Wait for user to release button.
while (READ_SWITCHES != 0x00); //See board.h for READ_SWITCHES define.
}
}
void gyro_init(void)
{
/* Initialize TWI master. */
TWI_MasterInit(&twiMaster,
&QYRO_TWI,
TWI_MASTER_INTLVL_HI_gc,
TWI_BAUDSETTING);
/* Enable LO interrupt level. */
PMIC.CTRL |= PMIC_LOLVLEN_bm;
sei();
//gyro_write_byte(ACCEL_ADDRESS,CTRL_REG1,0b01110111);
gyro_write_byte(GYRO_ADDRESS,CTRL_REG1,0b11111100); // ODR = 760Hz, Cut-off = 100, y axis enabled
gyro_write_byte(GYRO_ADDRESS,CTRL_REG4,0b00110000); // 2000 dps
gyro_write_byte(GYRO_ADDRESS,CTRL_REG5,0b00010000);
}
/*! /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 */
}
}
void initTWI()
{
TWI_MasterInit(&twiMaster, &TWIC, TWI_MASTER_INTLVL_LO_gc, TWI_BAUDSETTING);
}
extern void init_rtc()
{
TWI_MasterInit(100);
}
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;
}
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;
}
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);
}
}
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;
}
