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