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)); }