int main(void){
unsigned int temp_int;
unsigned char val = 0;
CTS_DDR_REG |= (1<<CTS_PIN); // set to output
CTS_PORT_REG |= (1<<CTS_PIN); // set high
RTS_DDR_REG &= ~(1<<CTS_PIN); // set to input
RTS_PORT_REG |= (1<<CTS_PIN); // enable pull-up
mod_led_init();
InitTWI();
initbootuart(); // Initialize UART.
/* Main loop */
while(true)
{
val = recchar(); // Wait for command character.
switch(val) {
case 'P':
case 'L':
case 'E':
sendchar('\r');
break;
// Read lock byte -> execute command
case 'r':
switch(command_char) {
case 'a':
// NOT SUPPORTED in new code.
read_and_send( TWI_CMD_AVERSION );
break;
case 'b':
// NOT SUPPORTED in new code.
read_and_send( TWI_CMD_BVERSION );
break;
case 'd':
// Read CRCHI
sendchar(CRC_HI);
break;
case 'e':
// Read CRCLO
sendchar(CRC_LO);
break;
case 'f':
// Status condition
// NOT SUPPORTED in new code.
read_and_send(TWI_CMD_GETERRCONDN);
break;
default:
sendchar(0xFF);
break;
}
break;
case 'l':
// Write lock byte -> load command. NOT SUPPORTED in new code.
// NOTE: This looks like a hijacked command to do a CRC check.
command_char = recchar();
#if 0
if( command_char == 'c' )
{
send_command( TWI_CMD_CRCCHECK );
read_from_slave();
CRC_HI= statusCode;
read_from_slave();
CRC_LO = statusCode;
}
#endif
sendchar('\r');
break;
case 'N':
// Read high fuse bits -> BVERSION
read_and_send( TWI_CMD_BVERSION );
break;
case 'F':
// Low Fuse Bits -> AVERSION
read_and_send( TWI_CMD_AVERSION );
break;
case 'a':
sendchar('Y'); // Yes, we do auto-increment.
break;
case 'A':
addr =(recchar()<<8) | recchar(); // Read address high and low byte.
if(addr > MAX__APP_ADDR) over_size_flag = 1;
//+ 15mar17 ndp - send address to Slave.
slaveCmdBuff[0] = CMD_RECV_ADRS;
slaveCmdBuff[1] = (uint8_t)((addr>>8) & 0x00FF); // AH
slaveCmdBuff[2] = (uint8_t)(addr & 0x00FF); // AL
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 3 );
//-
sendchar('\r'); // Send OK back.
break;
case 'e':
// Chip erase. NOT SUPPORTED in new code.
#if 0
runApp[0] = TWI_CMD_ERASEFLASH;
runApp[1] = TWI_CMD_ERASEFLASH;
get_slave_status();
success = MasterTransmit( SLAVE_ADDRESS, runApp, 2 );
#endif
sendchar('\r'); // Send OK back.
break;
case 'b':
// Check block load support.
sendchar('Y'); // Report block load supported.
sendchar((BLOCKSIZE>>8) & 0xFF); // MSB first.
sendchar(BLOCKSIZE&0xFF); // Report BLOCKSIZE (bytes).
over_size_flag = 0; // ndp 1-29-2017 fix
break;
case 'B':
// Start block load.
temp_int = (recchar()<<8) | recchar(); // Get block size.
val = recchar(); // Get memtype.
sendchar( BlockLoad(temp_int, val) ); // Block load.
// mod_led_toggle(4); // Need a short delay here.
pageBuffer[0] = CMD_RECV_DATA; // Address was sent in 'A' command service.
pageBuffer[1] = (uint8_t)(temp_int & 0x00FF); // NL..Only block size less than 256 supported.
// NOTE: Always sends PAGE_SIZE even if less data received from Host.
success = MasterTransmit( SLAVE_ADDRESS, pageBuffer, pageBuffer[1]+2 );
break;
case 'S':
// Return programmer identifier.
sendchar('A'); // Return 'AVRBOOT'.
sendchar('V'); // Software identifier (aka programmer signature) is always 7 characters.
sendchar('R');
sendchar('B');
sendchar('O');
sendchar('O');
sendchar('T');
reps =0;
break;
case 'V':
// Return software version.
// NOTE: TODO Should implement in new code.
// send_command(TWI_CMD_EXECUTEAPP);
// Disable bootloader mode for slave
sendchar('2');
sendchar('0');
break;
case 's':
// Return signature bytes [for the Target device ATtiny85].
slaveCmdBuff[0] = CMD_GET_SIG;
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 1 );
mod_led_toggle(200); // Need a short delay here to let Slave set up data.
(void) MasterReceive( SLAVE_ADDRESS, slaveCmdBuff, 3 );
sendchar( slaveCmdBuff[2] );
sendchar( slaveCmdBuff[1] );
sendchar( slaveCmdBuff[0] );
break;
/* Add missing command .. ndp 01-29-2017
* Return Flash Data.
*
* TODO: Need to read from Slave.
*/
case 'g':
temp_int = (recchar()<<8) | recchar(); // Get block size.
val = recchar(); // Get mem type.
// NOTE: Address was sent in 'A' command process.
slaveCmdBuff[0] = CMD_GET_DATA;
slaveCmdBuff[1] = (uint8_t)(temp_int & 0x00FF);
(void) MasterTransmit( SLAVE_ADDRESS, slaveCmdBuff, 2 );
mod_led_toggle(200); // Need a short delay here to let Slave set up data.
(void) MasterReceive( SLAVE_ADDRESS, pageBuffer, (temp_int & 0x00FF) );
for(int i=0; i<temp_int; ++i)
{
sendchar( pageBuffer[i] );
}
break;
default:
if(val != 0x1b) { // If not ESC, then it is unrecognized...
sendchar('?');
}
break;
} // end: switch()
} // end: while(true)
} // end: main
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;
}
void phex1(unsigned char c)
{
if (!print_enable) return;
sendchar(c + ((c < 10) ? '0' : 'A' - 10));
}
