void skyBin_fillBuffer(void)
{
uint8_t data_gps;
static uint8_t i=0;
static bool gps_inData = false;
static uint8_t skyBinBuffer[100];
if (USART_RXBufferData_Available(&USART_data)) {
data_gps = USART_RXBuffer_GetByte(&USART_data);
if (skyBin_checkOutpStart(data_gps) && gps_inData==false)
gps_inData = true;
else if (skyBin_checkOutpStop(data_gps) && gps_inData==true) {
gps_inData = false;
i=0;
if (skyBin_checkValid(skyBinBuffer)) {
PORTE.OUTTGL =PIN0_bm;
skyBin_decode(skyBinBuffer);
skyBin_toString();
skyBin_calcSpeed();
printAll_UART();
}
} else if (gps_inData == true)
skyBinBuffer[i++] = data_gps;
else {
//USART_Puts("Else in loop: ");
}
}
}
/*! \brief Example application.
*
* Example application. This example configures USARTE0 for with the parameters:
* - 8 bit character size
* - No parity
* - 1 stop bit
* - 9600 Baud
*
* This function then sends three bytes and tests if the received data is
* equal to the sent data. The code can be tested by connecting PC3 to PC2. If
* the variable 'success' is true at the end of the function, the three bytes
* have been successfully sent and received.
*/
int main(void)
{
/* counter variable. */
uint8_t i;
ConfigClockSystem();
ConfigUart();
/* Enable global interrupts. */
sei();
/* Fetch received data as it is received. */
i = 0;
while (i != '@') {
if (USART_RXBufferData_Available(&USART_data)) {
i = USART_RXBuffer_GetByte(&USART_data);
USART_TXBuffer_PutByte(&USART_data, i);
}
}
/* Disable both RX and TX. */
USART_Rx_Disable(&USART);
USART_Tx_Disable(&USART);
return 0;
}
void naiboard_process_stdin(void) {
uint8_t stdin_in = 0;
static char stdin_buffer[STDINBUFFERSIZE];
static uint8_t stdin_bufferpos;
while (USART_RXBufferData_Available(&naiboard_uart)) {
stdin_in = USART_RXBuffer_GetByte(&naiboard_uart);
if (stdin_in == '\n' || (stdin_in == '\r' && stdin_bufferpos == 0)) {
printf_P(PSTR("\n"));
continue;
}
if (stdin_bufferpos+1 == STDINBUFFERSIZE && stdin_in != '\b' && stdin_in != 0x7f && stdin_in != '\r')
continue;
if (stdin_in == '\b' || stdin_in == 0x7f) { // 0x7f - backspace
printf_P(PSTR("%c %c"), stdin_in, stdin_in);
if (stdin_bufferpos > 0)
stdin_bufferpos--;
continue;
}
printf_P(PSTR("%c"), stdin_in);
if (stdin_in == '\r') {
printf_P(PSTR("\n"));
stdin_buffer[stdin_bufferpos] = 0;
stdin_bufferpos = 0;
nai_console_processcommand(stdin_buffer);
} else
stdin_buffer[stdin_bufferpos++] = stdin_in;
}
}
char Comm::getchr(void)
{
char res = 0U;
if (USART_RXBufferData_Available(&USART_data)) {
res = USART_RXBuffer_GetByte(&USART_data);
}
return (res);
}
int usart_getchar (FILE *stream)
{
(void) stream;
if (USART_RXBufferData_Available (&usart_stdio_data)) {
return USART_RXBuffer_GetByte (&usart_stdio_data);
} else {
return -1;
}
}
/** Main program entry point. This routine contains the overall program flow, including initial
* setup of all components and the main program loop.
*/
int main(void)
{
SetupHardware();
GlobalInterruptEnable();
uint8_t sending = 0;
for (;;) {
while (1) {
int16_t ReceivedByte = CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface);
if (ReceivedByte < 0)
break;
if (!configured) continue;
if (!sending) {
PORTC.OUTSET = PIN1_bm;
sending = 1;
}
PORTD.OUTTGL = PIN5_bm;
while(!USART_IsTXDataRegisterEmpty(&USART));
USART_PutChar(&USART, ReceivedByte & 0xff);
}
if (sending) {
USART_ClearTXComplete(&USART);
while(!USART_IsTXComplete(&USART));
PORTC.OUTCLR = PIN1_bm;
sending = 0;
}
Endpoint_SelectEndpoint(VirtualSerial_CDC_Interface.Config.DataINEndpoint.Address);
/* Check if a packet is already enqueued to the host - if so, we shouldn't try to send more data
* until it completes as there is a chance nothing is listening and a lengthy timeout could occur */
if (configured && Endpoint_IsINReady()) {
uint8_t maxbytes = CDC_TXRX_EPSIZE;
while (USART_RXBufferData_Available(&USART_data) && maxbytes-->0) {
uint8_t b = USART_RXBuffer_GetByte(&USART_data);
CDC_Device_SendByte(&VirtualSerial_CDC_Interface, b);
PORTD.OUTTGL = PIN5_bm;
}
}
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
if (loop++) continue;
if (!configured) continue;
PORTD.OUTTGL = PIN5_bm;
}
}
/************************************************************************
* \brief Telemetry RX task.
************************************************************************/
void telemetry_rx_task(void)
{
uint8_t received_byte;
while (USART_RXBufferData_Available(&telemetry_usart_data))
{
received_byte = USART_RXBuffer_GetByte(&telemetry_usart_data);
VTOLLinkConnection connection = (VTOLLinkConnection)&vtolLinkConnection;
vtol_link_process_input_stream(connection, received_byte);
}
}
/*! \brief Get a byte from the circular receive buffer
*
* \param uart pointer to UART datastructure with buffers
*
* \return received byte from circulair buffer (low byte) or
* UART_NO_DATA if buffer is empty
*/
uint16_t uart_getc(USART_data_t *uart)
{
uint8_t data;
if ( ! USART_RXBufferData_Available(uart) ) {
return UART_NO_DATA;
}
data = USART_RXBuffer_GetByte(uart);
return (data & 0x00FF);
}
void dump_sensors(uint16_t digital, uint8_t analog) {
uint8_t pin;
uint8_t value;
while(1) {
if (USART_RXBufferData_Available(&BT_data) && '*' == USART_RXBuffer_GetByte(&BT_data))
return;
bt_putchar(':');
for (pin=0; pin<10; pin++)
if (digital & (1<<pin))
bt_putchar(read_input('0'+pin)+48);
for (pin=0; pin<6; pin++)
if (analog & (1<<pin)) {
value = read_analog(pgm_read_byte_near(muxPosPins+pin), 0);
put_hex_nibble(value>>4);
put_hex_nibble(value&0xF);
}
_delay_ms(5);
}
void hagisonic_module()
{
// unsigned int hagisonic_string_length=0;
char * keyword_pt, *temp_pt;
char temp_char1, temp_char2;
//int i_flag=0;
static unsigned int hagisonic_i=0;
while (USART_RXBufferData_Available(&USARTD0_data))
{
hagisonic_string[hagisonic_i] = USART_RXBuffer_GetByte(&USARTD0_data);
hagisonic_i++;
}
i_cnt=hagisonic_i;
//hagisonic_string[hagisonic_i]=0xFF;
keyword_pt=hagisonic_string;
//hagisonic_i=0;
for(int i=0; i<(hagisonic_i/14); i++)
{
while(*keyword_pt!=0x02)
{
keyword_pt++;
}
if(*(keyword_pt+13)==0x03)
{
temp_pt=keyword_pt;
temp_char1=*temp_pt;
start_bit=(unsigned int)(temp_char1&0b11111111);
/*
//store theta data
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
H_angle=((int)temp_char1)*256;
H_angle+=(int)temp_char2;
//store x data
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
H_x= ((int)temp_char1)*256;
H_x+=(int)temp_char2;
//store y data
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
H_y=((int)temp_char1)*256;
H_y+=(int)temp_char2;
*/
//store theta data
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
H_angle=((unsigned int)temp_char1)*256;
H_angle+=(unsigned int)temp_char2;
//store x data
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
H_x= ((unsigned int)temp_char1)*256;
H_x+=(unsigned int)temp_char2;
//store y data
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
H_y=((unsigned int)temp_char1)*256;
H_y+=(unsigned int)temp_char2;
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
Encoder_angle=((unsigned int)temp_char1)*256;
Encoder_angle+=(unsigned int)temp_char2;
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
Encoder_x= ((unsigned int)temp_char1)*256;
Encoder_x+=(unsigned int)temp_char2;
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
Encoder_y=((unsigned int)temp_char1)*256;
Encoder_y+=(unsigned int)temp_char2;
temp_pt++;
temp_char1=*temp_pt;
end_bit=(unsigned int)(temp_char1&0b11111111);
//하기소닉 모듈에서 들어온 x,y,theta 데이터를 서울대 보드에서 fusion하는 변수에 저장
Z_x=(double)(H_x)/1000.0;
Z_y=(double)(H_y)/1000.0;
Z_a=(double)(H_angle)/100.0;
//Z_q=cos(Z_q/2);
m_visionUpdateFlag=1;
}
}
hagisonic_i=0;
}
void host_interface()
{
static int initial_ENC=1;
// static int encoder_on=0;
unsigned int host_string_length=0;
char * keyword_pt;
unsigned char temp_char1, temp_char2, temp_char3;
char * temp_pt;
int encoder_on;
// static int oldTimeDelay=0;
static int host_i=0;
// unsigned char XYZ_buffer[100];
while (USART_RXBufferData_Available(&USARTC0_data))
{
host_string[host_i] = USART_RXBuffer_GetByte(&USARTC0_data);
host_i++;
}
host_string[host_i]='\0';
keyword_pt=strstr(host_string, "[");
temp_pt=keyword_pt;
host_string_length=strlen(keyword_pt);
if(keyword_pt==NULL){
host_i=0;
host_string[0]='\0';
}
temp_pt++;
if(*temp_pt=='E')
{
if(host_string_length>9 && *(temp_pt+8)==']')
{
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
temp_pt++;
temp_char3=*temp_pt;
// enc_left=temp_char1<<16+temp_char2<<8+temp_char3;
if(temp_char1=='N') encoder_on=0;
else encoder_on=1;
if(encoder_on){
m_LeftEncoder= (unsigned int)(temp_char1&0b00011111)<<11;
m_LeftEncoder+=(unsigned int)(temp_char2&0b00011111)<<6;
m_LeftEncoder+=(unsigned int)(temp_char3&0b00111111);
// enc_right=(keyword_pt+6)*256*256+(keyword_pt+7)*256+(keyword_pt+8);
temp_pt++;
temp_pt++;
temp_char1=*temp_pt;
temp_pt++;
temp_char2=*temp_pt;
temp_pt++;
temp_char3=*temp_pt;
m_RightEncoder= (unsigned int)(temp_char1&0b00011111)<<11;
m_RightEncoder+=(unsigned int)(temp_char2&0b00011111)<<6;
m_RightEncoder+=(unsigned int)(temp_char3&0b00111111);
// image_x=(keyword_pt+10)*256+(keyword_pt+11);
H_m_LeftEncoder=m_LeftEncoder/100;
H_m_RightEncoder=m_RightEncoder/100;
while(H_m_LeftEncoder>255) H_m_LeftEncoder-=256;
while(H_m_RightEncoder>255) H_m_RightEncoder-=256;
if(encoder_first==1)
{
H_m_LeftEncoder_first=(char)(H_m_LeftEncoder);
H_m_RightEncoder_first=(char)(H_m_RightEncoder);
//encoder_first=0;
}
H_encoderL_BM2=(char)((char)(H_m_LeftEncoder)-H_m_LeftEncoder_first);
H_encoderR_BM2=(char)((char)(H_m_RightEncoder)-H_m_RightEncoder_first);
H_encoderR_BM2=-H_encoderR_BM2;
if(encoder_first==1)
{
m_encoder_first_L=(char)((char)(m_LeftEncoder/256));
m_encoder_first_R=(char)((char)(m_RightEncoder/256));
//encoder_first=0;
}
H_encoderL=(char)((char)(m_LeftEncoder/256)-m_encoder_first_L);
H_encoderR=(char)((char)(m_RightEncoder/256)-m_encoder_first_R);
//H_encoderL=(char)(256-H_encoderL);
//H_encoderR=(char)(256-H_encoderR);
//H_encoderL=(char)(255-m_LeftEncoder/256);
//H_encoderR=(char)(255-m_RightEncoder/256);
//int lt=(int)(m_LeftEncoder-m_prevDLeft);
lt=m_LeftEncoder -m_prevDLeft;
rt=m_RightEncoder-m_prevDRight;
/*
if(lt>32768) m_dL=-(char)(65536-lt)/256;
else m_dL=(double)lt/256;
if(rt>32768) m_dR=-(double)(65536-rt)/256;
else m_dR=(double)rt/256;
*/
double m_dLeftInc_H=0;
double m_dRightInc_H=0;
if(lt>32768) m_dLeftInc_H=-(double)(65536-lt);
else m_dLeftInc_H=(double)lt;
if(rt>32768) m_dRightInc_H=-(double)(65536-rt);
else m_dRightInc_H=(double)rt;
if(lt>32768) m_dLeftInc=-(double)(65536-lt)/REDUCTION_ENCODER;
else m_dLeftInc=(double)lt/REDUCTION_ENCODER;
if(rt>32768) m_dRightInc=-(double)(65536-rt)/REDUCTION_ENCODER;
else m_dRightInc=(double)rt/REDUCTION_ENCODER;
// m_dLeftInc_sum +=m_dLeftInc;
// m_dRightInc_sum+=m_dRightInc;
double H_enc_Left_diff=0;
double H_enc_Right_diff=0;
H_enc_Left_diff =100.0/36.0*1000.0/1025.2*m_dLeftInc_H; //예전에 100.0/16.1로 함.
H_enc_Right_diff=100.0/36.0*1000.0/1025.2*m_dRightInc_H;
// H_enc_Left_diff =150.0/55.4*m_dLeftInc_H; //1.5 m 기준
// H_enc_Right_diff=150.0/55.4*m_dRightInc_H;
// H_enc_Left_diff =100.0/16.1*m_dLeftInc_H;
// H_enc_Right_diff=100.0/16.1*m_dRightInc_H;
// H_enc_Left_diff =100.0/37.1*m_dLeftInc_H;// 36.9*m_dLeftInc_H; //1 m 기준
// H_enc_Right_diff=100.0/37.1*m_dRightInc_H;//36.9*m_dRightInc_H;
// H_enc_Left_diff =m_dLeftInc_H;
// H_enc_Right_diff=m_dRightInc_H;
if(encoder_first==1)
{
H_enc_Left_diff_first=(char)((char)(H_enc_Left_diff/130.6)); // 130.6=78386/600
H_enc_Right_diff_first=(char)((char)(H_enc_Right_diff/130.6)); //예전에 306.2로 함. (78386/256)
// H_enc_Left_diff_first=(char)((char)(H_enc_Left_diff/306.2));
// H_enc_Right_diff_first=(char)((char)(H_enc_Right_diff/306.2));
encoder_first=0;
}
H_encoderL_sum+=H_enc_Left_diff;
H_encoderR_sum+=H_enc_Right_diff;
H_encoderL_BM=(char)((char)(H_encoderL_sum/130.6)-H_enc_Left_diff_first);
H_encoderR_BM=(char)((char)(H_encoderR_sum/130.6)-H_enc_Right_diff_first);
// H_encoderL_BM=(char)((char)(H_encoderL_sum/306.2)-H_enc_Left_diff_first);
// H_encoderR_BM=(char)((char)(H_encoderR_sum/306.2)-H_enc_Right_diff_first);
//H_encoderL_BM=(char)(H_encoderL_BM);
//H_encoderR_BM=(char)(H_encoderR_BM);
//H_encoderR_BM=H_encoderR_BM;
/*
if(lt>32768) m_dLeftInc_hagisonic=-(double)(65536-lt)/45.0;
else m_dLeftInc_hagisonic=(double)lt/45.0;
if(rt>32768) m_dRightInc_hagisonic=-(double)(65536-rt)/45.0;
else m_dRightInc_hagisonic=(double)rt/45.0;
Encoder_diff_Left=(char)m_dLeftInc_hagisonic;
Encoder_diff_Right=(char)m_dRightInc_hagisonic;
*/
m_dLeftInc_hagisonic=m_dLeftInc/0.00037699;
m_dRightInc_hagisonic=m_dRightInc/0.00037699;
//m_dLeftInc_hagisonic +=m_dLeftInc;
//m_dRightInc_hagisonic+=m_dRightInc;
Encoder_diff_Left=(char)m_dLeftInc_hagisonic;
Encoder_diff_Right=(char)m_dRightInc_hagisonic;
m_prevDLeft=m_LeftEncoder;
m_prevDRight=m_RightEncoder;
g_encoder_flag=1;
//g_encoder_flag=encoder_on;
if(initial_ENC){
m_dLeftInc=0;
m_dRightInc=0;
initial_ENC=0;
}
}
else{
m_dLeftInc=0;
m_dRightInc=0;
initial_ENC=1;
}
host_i=host_string_length-10;
strcpy(host_string,keyword_pt+10);
}
}
}
int main(void)
{
SetXMEGA32MhzCalibrated(); //Set XMega to user 32Mhz internal oscillator with 32Khz crystal calibration
///////Setup Inputs and Outputs///////
PORTC.DIRSET = (PIN5_bm | PIN6_bm | PIN7_bm | PIN3_bm); //Sets outputs on port C
PORTC.DIRCLR = PIN2_bm; //Sets inputs on PORT C
PORTA.DIRCLR = XBEEDIO0;
PORTE.DIRSET = PIN3_bm; //Sets inputs on PORTA
///////Initialize Serial Communcations///////
SetupPCComms(); //Initializes PC Communications at 9600 baud0
_delay_ms(500); //Delay to make sabertooth initialize
Sabertooth DriveSaber(&USARTD0, &PORTD); //Initializes Sabertooth Communications at 9600 Baud
//////////////////Timers///////////////
TCC0.CTRLA = TC_CLKSEL_DIV1024_gc; //31250 counts per second with 32Mhz Processor
TCC0.CTRLB = TC_WGMODE_NORMAL_gc;
TCC0.PER = 15625;
TCC0.INTCTRLA = TC_OVFINTLVL_LO_gc;
TCD0.CTRLA = TC_CLKSEL_DIV1024_gc; //31250 counts per second with 32Mhz Processor
TCD0.CTRLB = TC_WGMODE_NORMAL_gc;
TCD0.PER = 31250;
TCD0.INTCTRLA = TC_OVFINTLVL_LO_gc;
///////////////////Timers//////////////
sei(); //Enables global interrupts so the interrupt serial can work
////Semi-global vars//////
unsigned char BufferIdx = 0;
const char XMegaID[] = "ID: MainDrive\r\n";
enum MicroState{
WaitForHost,
Driving
}XMegaState = WaitForHost;
while(1){
switch(XMegaState){
case WaitForHost:
for(int i = 0 ; XMegaID[i] != '\0'; i++){
while(!USART_IsTXDataRegisterEmpty(&USARTC0));
USART_PutChar(&USARTC0, XMegaID[i]);
}
_delay_ms(500);
if(USART_RXBufferData_Available(&USART_PC_Data)){
if(USART_RXBuffer_GetByte(&USART_PC_Data) == 'r'){
XMegaState = Driving;
//DriveSaber.ResetSaber();
FlushSerialBuffer(&USART_PC_Data);
USART_PutChar(&USARTC0, 'r');
}
}
TimePrevious = TimeSinceInit;
break;
case Driving:
if(USART_RXBufferData_Available(&USART_PC_Data)){
receiveArray[BufferIdx] = USART_RXBuffer_GetByte(&USART_PC_Data);
BufferIdx++;
}
if(BufferIdx == RECEIVE_PACKET_SIZE){
FlushSerialBuffer(&USART_PC_Data);
if(IsRoving){
if(receiveArray[4] == PCComsChecksum(receiveArray[1], receiveArray[2], receiveArray[3])){
DriveSaber.ParsePacket(receiveArray[2], receiveArray[3]);
}
}else if(!IsRoving){
DriveSaber.ParsePacket(127, 127);
}
BufferIdx = 0;
SendDriveControlStatus(&USARTC0, IsRoving, false);
TimePrevious = TimeSinceInit;
}
if((TimeSinceInit - TimePrevious) > TIMEOUTMAX){
DriveSaber.StopAll();
XMegaState = WaitForHost;
TimePrevious = TimeSinceInit;
}
break;
};
/*
if(!IsRoving){
DriveSaber.StopAll();
}
*/
if((PORTA.IN & XBEEDIO0)){
ERROR_CLR();
IsRoving = true;
}else if((!(PORTA.IN & XBEEDIO0))){
ERROR_SET();
IsRoving = false;
}
}
}
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)
{
uint8_t sensor; // Stores analog readings for transmission
int i; // multipurpose counter
char choice = 0; // Holds the top-level menu character
// int red = 0; // For the red LED intensity
// int green = 0; // For the green LED intensity
// int blue = 0; // For the blue LED intensity
uint8_t exit = 0; // Flag that gets set if we need to go back to idle mode.
unsigned int temph=0; // Temporary variable (typically stores high byte of a 16-bit int )
unsigned int templ=0; // Temporary variable (typically stores low byte of a 16-bit int)
uint8_t location = 0; // Holds the EEPROM location of a stored IR signal
// unsigned int frequency = 0; // For PWM control - PWM frequency, also used to set buzzer frequency
// int amplitude;
int channel;
unsigned int duty; // For PWM control - PWM duty cycle
int baud; // Baud rate selection for auxiliary UART
char scale; // For auxiliary UART
long int time_out=0; // Counter which counts to a preset level corresponding to roughly 1 minute
// Initialize system
// Turn off JTAG to save a bit of battery life
CCP = CCP_IOREG_gc;
MCU.MCUCR = 1;
init_clock();
init_led();
init_adc();
init_ir();
init_BT();
init_dac();
init_buzzer();
initAccel();
init_aux_uart(131, -3); // Set the auxiliary uart to 115200 8n1
EEPROM_DisableMapping();
// Enable global interrupts
sei();
// Do the following indefinitely
while(1) {
// Turn off LED
set_led(0,0,0);
// Reset flags (in case this isn't the first time through the loop)
exit = 0;
choice = 0;
time_out = 0;
stop_ir_timer();
// Sing a BL song in idle mode so you can be found. Stop as soon as you get a *
while(choice != 42) {
bt_putchar('B');
bt_putchar('L');
if (USART_RXBufferData_Available(&BT_data)) {
choice = USART_RXBuffer_GetByte(&BT_data);
if (choice == 128) {
// Something is trying to connect directly to an iRobot
set_aux_baud_rate( ROOMBA_UART_SETTINGS ); // depends on model
aux_putchar( 128); // pass through to iRobot
serial_bridge(); // currently never returns
}
else if (choice == 0) {
// Something may be trying to connect directly to a MindFlex headset
set_aux_baud_rate( 135, -2); // 57600
aux_putchar( 0); // pass through to headset
serial_bridge(); // currently never returns;
}
else {
bt_putchar(choice);
}
}
if (choice != 42)
_delay_ms(500);
}
// Active part of the program - listens for commands and responds as necessary
while(exit == 0) {
// Checks if we haven't heard anything for a long time, in which case we exit loop and go back to idle mode
time_out++;
// Corresponds to roughly 60 seconds
if(time_out > 33840000) {
exit = 1;
}
// Check for a command character
if (USART_RXBufferData_Available(&BT_data)) {
choice = USART_RXBuffer_GetByte(&BT_data);
}
else {
choice = 0;
}
// If it exists, act on command
if(choice != 0) {
// Reset the time out
time_out = 0;
// Return the command so the host knows we got it
bt_putchar(choice);
// Giant switch statement to decide what to do with the command
switch(choice) {
// Return the currect accelerometer data - X, Y, Z, and status (contains tapped and shaken bits)
case 'A':
updateAccel();
bt_putchar(_acc.x);
bt_putchar(_acc.y);
bt_putchar(_acc.z);
bt_putchar(_acc.status);
break;
// Set the buzzer
case 'B': // frequency_divider(2)
if (get_arguments(2)) {
set_buzzer(GET_16BIT_ARGUMENT(0));
}
break;
// Turn off the buzzer
case 'b':
turn_off_buzzer();
break;
// Returns the value of the light sensor
case 'L':
sensor = read_analog(LIGHT, 0);
bt_putchar(sensor);
break;
// Returns the Xmegas internal temperature read - this is undocumented because the value returned is very erratic
case 'T':
sensor = read_internal_temperature();
bt_putchar(sensor);
break;
// Returns the battery voltage
case 'V':
sensor = read_analog(BATT_VOLT, 0);
bt_putchar(sensor);
break;
// Differential ADC mode
case 'D': // active(1) numgainstages(1)
if (get_arguments(2)) {
if (arguments[0] == '0') {
adcResolution = ADC_RESOLUTION_8BIT_gc;
adcConvMode = ADC_ConvMode_Unsigned;
adcGain = ADC_DRIVER_CH_GAIN_NONE;
adcInputMode = ADC_CH_INPUTMODE_SINGLEENDED_gc;
init_adc();
}
else if (arguments[0] == '1') {
adcResolution = ADC_RESOLUTION_12BIT_gc;
adcConvMode = ADC_ConvMode_Signed;
if (arguments[1] >= '1' && arguments[1] <= '6') { // number of gain stages
adcGain = (arguments[1] - '0') << ADC_CH_GAINFAC_gp;
adcInputMode = ADC_CH_INPUTMODE_DIFFWGAIN_gc;
init_adc();
}
else if (arguments[1] == '0') {
adcGain = ADC_DRIVER_CH_GAIN_NONE;
adcInputMode = ADC_CH_INPUTMODE_DIFF_gc;
init_adc();
}
else {
err();
}
}
else {
err();
}
}
break;
// Returns the readings on all six ADC ports
case 'X':
for (i=0; i<6; i++) {
sensor = read_analog(pgm_read_byte_near(muxPosPins+i), 0);
bt_putchar(sensor);
}
break;
// Returns differential measurement on pair of ADC ports
// Assumes we are in differential mode
case 'x':
if (get_arguments(2)) {
i = read_differential(arguments[0], arguments[1]);
bt_putchar(0xFF&(i >> 8));
bt_putchar(0xFF&i);
}
break;
case 'e': // this is a bit of testing code, which will eventually be changed
// do not rely on it
if (get_arguments(2)) {
char a1 = arguments[0];
char a2 = arguments[1];
while(1) {
_delay_ms(2);
i = read_differential(a1, a2);
itoa((i<<4)>>4, (char*)arguments, 10);
for (i=0; arguments[i]; i++)
bt_putchar(arguments[i]);
bt_putchar('\r');
bt_putchar('\n');
}
}
break;
// Sets the full-color LED
case 'O': // red(1) green(1) blue(1);
if (get_arguments(3)) {
set_led(arguments[0], arguments[1], arguments[2]);
}
break;
// Switches serial between irDA and standard serial
// Currently, this works over the same port numbers as
// standard serial, instead of using the IR receiver
// and IR LED. This may change when I get the IR receiver
// and LED working reliably.
case 'J':
temph = bt_getchar_timeout_echo();
if(/*temph == 256 || */ temph < '0' || temph > '1') {
err();
break;
}
set_irda_mode(temph - '0');
break;
// Sets up the IR transmitter with signal characteristics
case 'I':
init_ir();
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set the frequency of the IR carrier
robotData.frequency = ((temph)<<8) + templ;
set_ir_carrier(robotData.frequency);
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
else {
// Set the length of the start up pulses
robotData.startUpPulseLength = templ;
}
if(robotData.startUpPulseLength > 16) {
err();
break;
}
// Read in the start up pulse timing data
for(i=0; i < robotData.startUpPulseLength; i++) {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
robotData.startUpPulse[i] = ((temph)<<8) + templ;
}
if(temph == 256 || templ == 256) {
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set the bit encoding to one of four pre-determined settings (see protocol instructions for more information)
robotData.bitEncoding = templ;
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set the number of bits (and bytes) contained in an IR command
robotData.numBits = templ;
robotData.numBytes = (robotData.numBits-1)/8 + 1;
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set timing data for a high bit
robotData.highBitTime = ((temph)<<8) + templ;
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
// Set timing data for a low bit
robotData.lowBitTime = ((temph)<<8) + templ;
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set timing data for on or off
robotData.offTime = ((temph)<<8) + templ;
break;
// Transmit an IR signal according to the previously determined configuration
case 'i':
init_ir();
// Get the signal data as one or more bytes
for(i = 0; i < robotData.numBytes; i++) {
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
robotData.irBytes[i] = templ;
}
if(templ == 256) {
break;
}
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Determine if the signal is repeated or not, and if so, with what frequency
robotData.repeatTime = ((temph)<<8) + templ;
if(robotData.repeatTime != 0) {
robotData.repeatFlag = 1;
}
else {
robotData.repeatFlag = 0;
}
// Startup timer interrupts
start_ir_timer();
break;
// Turn off any repeating IR signal
case '!':
robotData.repeatFlag = 0;
stop_ir_timer();
break;
// Capture a signal from the IR receiver
case 'R':
init_ir_read();
while(ir_read_flag!=0);
break;
// Store the captured signal in an EEPROM location
case 'S':
location = bt_getchar_timeout_echo()-48; // Subtracting 48 converts from ASCII to numeric numbers
if((location >= 0) && (location < 5) && (signal_count > 4)) {
write_data_to_eeprom(location);
}
else {
err();
}
break;
// Receive a raw IR signal over bluetooth and transmit it with the IR LED
case 's':
if(read_data_from_serial()) {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
// Set if the signal should repeat and if so, with what frequency
robotData.repeatTime = ((temph)<<8) + templ;
if(robotData.repeatTime != 0) {
robotData.repeatFlag = 1;
}
else {
robotData.repeatFlag = 0;
}
// Set frequency to 38KHz, since raw signals must have come from the receiver at some point
robotData.frequency = 0x0349;
robotData.startUpPulseLength = 0;
robotData.bitEncoding = 0x04;
start_ir_timer();
}
else {
err();
break;
}
break;
// Get a stored signal from an EEPROM location and transmit it over the IR LED (and repeat as desired)
case 'G':
location = bt_getchar_timeout_echo()-48;
if(location >= 0 && location < 5) {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
robotData.repeatTime = ((temph)<<8) + templ;
if(robotData.repeatTime != 0) {
robotData.repeatFlag = 1;
}
else {
robotData.repeatFlag = 0;
}
read_data_from_eeprom(location);
robotData.frequency = 0x0349;
robotData.startUpPulseLength = 0;
robotData.bitEncoding = 0x04;
start_ir_timer();
}
else {
err();
}
break;
// Get a stored signal from EEPROM and print it over bluetooth to the host
case 'g':
location = bt_getchar_timeout_echo()-48;
if(location >= 0 && location < 5) {
print_data_from_eeprom(location);
}
else {
err();
}
break;
// Output on digital I/O
case '>':
// Set port
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
// Get value
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
set_output(temph, (templ-48));
break;
// Input on digital I/O
case '<':
// Get port
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
// Get value (1 or 0)
templ = read_input(temph)+48;
bt_putchar(templ);
break;
// Configure PWM frequency
case 'P':
if (get_arguments(2))
pwm_frequency = GET_16BIT_ARGUMENT(0);
break;
// Set PWM duty cycle for a specific port
case 'p':
if (get_arguments(3)) {
set_pwm();
duty = GET_16BIT_ARGUMENT(1);
if (arguments[0] == '0')
set_pwm0(duty);
else if (arguments[1] == '1')
set_pwm1(duty);
// could add else err();, but original firmware doesn't do this
}
break;
// Set DAC voltage on one of the two DAC ports
case 'd':
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
if(temph == '0') {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
else {
set_dac0(temph);
}
}
else if(temph == '1') {
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
else {
set_dac1(temph);
}
}
break;
// Go back to idle mode so you can be found again. Should be sent at end of host program.
case 'Q':
exit = 1;
break;
// Human-readable uart speed setting
case 'u':
temph = bt_getchar_timeout_echo();
if(temph == 256) {
err();
break;
}
templ = bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
baud = ((temph)<<8) + templ;
switch(baud) {
case ('1'<<8) + '2': // 1200
baud = 6663;
scale = -2;
break;
case ('4'<<8) + '8': // 4800
baud = 3325;
scale = -3;
break;
case ('9'<<8) + '6': // 9600
baud = 829;
scale = -2;
break;
case ('1'<<8) + '9': // 19200
baud = 825;
scale = -3;
break;
case ('3'<<8) + '8': // 38400
baud = 204;
scale = -2;
break;
case ('5'<<8) + '7': // 57600
baud = 135;
scale = -2;
break;
case ('1'<<8) + '1': // 115200
baud = 131;
scale = -3;
break;
default:
err();
goto BAUD_DONE;
}
set_aux_baud_rate(baud, scale);
BAUD_DONE:
break;
// Configures the baud rate of the auxiliary UART
case 'C': // baud(2) scale(1)
// e.g., 9600 = C\x03\x3D\xFE
if (! get_arguments(3))
break;
bt_putchar(arguments[2]); // duplicate buggy behavior from official firmware; delete if unwanted
set_aux_baud_rate( GET_16BIT_ARGUMENT(0), arguments[2]);
break;
// BT-serial high speed bridge mode
case 'Z':
serial_bridge();
break;
case 'w': // channel(1) type(1) duty(1) amplitude(1) frequency(3)
// w0s\x20\xFF\x02\x00\x00
if (!get_arguments(7))
break;
if (arguments[0] < '0' || arguments[0] > '1' || arguments[2] > WAVEFORM_SIZE) {
err();
break;
}
play_wave_dac(arguments[0]-'0', (char)arguments[1], arguments[2], arguments[3], get_24bit_argument(4));
break;
case 'W': // channel(1) frequency(3) length(1) data(length)
if (!get_arguments(5))
break;
if (arguments[0] < '0' || arguments[0] > '1' || arguments[4] > WAVEFORM_SIZE || arguments[4] == 0 ) {
err();
break;
}
channel = arguments[0] - '0';
if (bt_to_buffer(waveform[channel], arguments[4])) {
disable_waveform(channel);
play_arb_wave(channel, waveform[channel], arguments[4], get_24bit_argument(1));
}
break;
case '@':
channel = bt_getchar_timeout_echo();
if (channel == '0')
disable_waveform0();
else if (channel == '1')
disable_waveform1();
else
err();
break;
case 'r':
i = USART_RXBufferData_AvailableCount(&AUX_data);
bt_putchar((uint8_t)(i+1));
while(i-- > 0) {
bt_putchar(USART_RXBuffer_GetByte(&AUX_data));
}
break;
// Transmit a stream of characters from bluetooth to auxiliary serial
case 't':
temph= bt_getchar_timeout_echo();
if (temph == 256) {
err();
break;
}
for(; temph>0; temph--) {
templ= bt_getchar_timeout_echo();
if(templ == 256) {
err();
break;
}
else {
aux_putchar( templ);
}
}
break;
default:
break;
}
}
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;
}
