The code to interface the Servos and the Roboard has already been explained on previouse post, so as brief explanation, this code colects the data from the servos, compose a pack with it and send it to the Roboard.
When the Roboard send a packet containing broadcast packets to the servos data, the CM5 will redirect the content to the servos.
Here is the first working version of the CM5 firmware.
See full article for the Code to Interface with the Servos
The Main cycle start and configure the CM5 and then enters on infinite cycle.
On the initialization the UARTs are initiated and its made a sweep to the existing servos on the bus, and is built a list with the IDs.
After this it enters on the infinite cycle, and this cycle tests if there is:
– a timeout on receiving data from servos,
– then any byte to receive from Servos,
– then any byte to send to servos,
– then any byte to receive from Roboard,
– and in last any byte to send to Roboard.
If there is no data to send to servos, it starts a loop of requesting data from servos to build the packet of data to send to the roboard. This loop consists on sending a query packet to the servo, requesting the position, and wait for the reply until timeout. If the servo replies within the timeout limit, we save the data and send the request to the next servo on the list, otherwise the request issent to the same servo.
This is made for each servo of the list. When we reach the total of servos, a packet with all the data is ready to be sent to the roboard.
The packet to the roboard is sent, and again it check if there is any packet incoming from the roboard, if there is, this super packet from roboard already contains the formated and ready to send broadcast packets, in case in being more than one broadcast in the same super packet, to the servos, so it send the broadcasts to the
servos and again enter on the data request loop to the servos.
/*********************************** * Last Change at 17-Oct-2010 * * 1st error on array changing and array selector *need to be made by the reader from PC and not by the sender to the servos * ************************************/ #include <avr/delay.h> #include <avr/io.h> #include <avr/interrupt.h> #include <ctype.h> #define BIT0 0x1 #define BIT1 0x2 #define BIT2 0x4 #define BIT3 0x8 #define BIT4 0x10 #define BIT5 0x20 #define BIT6 0x40 #define BIT7 0x80 #define PC_Comm 0 #define Servo_Comm 1 #define Led_Dir DDRC #define Led_Port PORTC #define POWERLed BIT0 #define TXLed BIT1 #define RXLed BIT2 #define AUXLed BIT3 #define MANAGELed BIT4 #define PROGRAMLed BIT5 #define PLAYLed BIT6 #define AX12_PING 0x01 #define AX12_READ 0x02 #define AX12_WRITE 0x03 #define AX12_REG 0x04 #define AX12_ACTION 0x05 #define AX12_RESET 0x06 #define AX12_SYNC_WR 0x83 #define SERVOBuffer_MAX_SIZE 256 typedef struct SERVO_DATA_STRUCT { uint8_t u8ServoID; uint8_t u8ServoInstruction; uint8_t u8ServoDataLength; uint8_t u8ServoData[SERVOBuffer_MAX_SIZE]; } stServo_Data; #define HalfDuplexTransceiver_Port PORTE #define HalfDuplexTransceiver_Dir DDRE #define HalfDuplexTransceiver_PinOUT BIT2 #define HalfDuplexTransceiver_PinIN BIT3 #define bit(param) (1<<(param)) #define UART0_Baudrate 1000000 //#define UART1_Baudrate 500000 #define UART1_Baudrate 115200 //#define UART1_Baudrate 1000000 unsigned char ReadyToSendToUART0=1; unsigned char ReadyToSendToUART1=1; #define ENABLE_UDRIE0 ReadyToSendToUART0=1//(UCSR0B |= bit(UDRIE)) #define ENABLE_UDRIE1 ReadyToSendToUART1=0//(UCSR1B |= bit(UDRIE)) #define DISABLE_UDRIE0 ReadyToSendToUART0=0//(UCSR0B &= ~bit(UDRIE)) #define DISABLE_UDRIE1 ReadyToSendToUART1=0//(UCSR1B &= ~bit(UDRIE)) /*****************SYSTEM DEFINES*********************/ #define MAX_SERVOS_ON_BUS 30 #define MAX_DEVICE_ID 253 #define MAX_SERVO_REPLY_TIMEOUT 5000 #define UART0TX \ HalfDuplexTransceiver_Port |= HalfDuplexTransceiver_PinOUT; \ HalfDuplexTransceiver_Port &= ~HalfDuplexTransceiver_PinIN; \ UART0_TXRX_state = 0; #define UART0RX \ HalfDuplexTransceiver_Port |= HalfDuplexTransceiver_PinIN; \ HalfDuplexTransceiver_Port &= ~HalfDuplexTransceiver_PinOUT; \ UART0_TXRX_state = 1; #define LEDAUX_ON \ Led_Port &= ~(AUXLed); #define LEDAUX_OFF \ Led_Port |= AUXLed; // servo broadcast double buffer. 1- continously broadcast. 2- from PC unsigned char UART0_TX_brdcst[2][256]; unsigned char* UART0_TX_brdcst_last=0; // pointer to last byte. set to 0 unless new packet just received unsigned char* UART0_TX_brdcst_first=0; // pointer to first byte. set to 0 unless new packet just received // current buffer of the two, being sent unsigned char UART0_TX_brdcst_outbuf=0; // pointer to last byte of servo broadcast buffer (initially don't broadcast) //unsigned char* UART0_TX_Qbuf_last = UART0_TX_Bbuf; // servo query buffer (static buffer except for servo ID and checksum) unsigned char UART0_TX_Qbuf[8] = {0xff, 0xff, 0, 4, AX12_READ, 0x24, 2, 0}; // length(4) + instruction(2) + 36 + 2 --> to create checksum add ID and ~not the result const unsigned char UART0_TX_Qbuf_checksum = 0x4+AX12_READ+0x24+0x2/*+ID*/; // pointer to last byte of servo query buffer const unsigned char* UART0_TX_Qbuf_last = &UART0_TX_Qbuf[7];//UART0_TX_Qbuf + sizeof(UART0_TX_Qbuf)-1; // PC broadcast double buffer. 1- being sent. 2- in preparation as servos respond unsigned char UART1_TX_buf[2][256]; // current buffer of the two, being sent unsigned char UART1_TX_buf_current=0; // pointer to last byte of PC current buffer unsigned char* UART1_TX_buf_last; // List of servos found by initial scan unsigned char Servos[MAX_SERVOS_ON_BUS]; // number of servos in the list unsigned char Servos_count; unsigned char UART1_TX_buf_sending=0; unsigned char UART1_TX_buf_ready=0; unsigned char UART0_TX_Bbuf_current=0; unsigned char UART0_TXRX_state=0; void find_servos_list(void); uint8_t InitSerialCommunications(uint32_t PCBaudrate, uint32_t ServoBaudrate); void crash(void); void wait_for_interrupts(void); void SendServoByte(void); void SendPCByte(void); #define MotorNextRequestDelay 150 /*SIGNAL(USART0_UDRE_vect)* / void SignalUSART0_UDRE(void){ // UART0_TX UDRIE interrupt (not TXC !) tx for SERVOs SendServoByte(); }*/ /* SIGNAL(USART0_TXC_vect){ // UART0_TX TXC interrupt (not URDIE !) tx for SERVOs Led_Port |= AUXLed; UART0_Receive(); UCSR0A|=bit(TXC); }*/ /*SIGNAL(USART1_UDRE_vect)* /void SignalUSART1_UDRE(void){ // UART0_TX UDRIE interrupt (not TXC !) tx for PC SendPCByte(); }*/ void PCSend(void) { static unsigned char* p; static unsigned char state = 1; static unsigned char* last; switch (state) { case 0: Led_Port &= ~(TXLed); UDR1 = *p++; if (p == last) { state = 2; } return; case 1: Led_Port &= ~(TXLed); // UART1_TX_buf_sending has been set to 1 in the UART0_RX() // this is the first byte to be sent in the new buffer prepared by UART0_RX() p = UART1_TX_buf[1-UART1_TX_buf_current]; last = p + p[2] + 4; UDR1 = *p++; state = 0; return; case 2: // don't send any more bytes.. waiting for reply from servo state = 1; UART1_TX_buf_sending = 0; // sends a message to UART0_RX() //Led_Port |= (PROGRAMLed); DISABLE_UDRIE1; // disable empty-buffer interrupts for TX1 Led_Port |= TXLed; return; } } static unsigned debug_timer=0; ///unsigned int PackReceiverCounter=0; ///unsigned char ReceivedState=1; static unsigned char servo = 0; void ServoSend(void) { static unsigned char *p = 0; static unsigned char *last = 0; static unsigned char state = 0; //static unsigned char Servos_checksum=0; switch (state) { case 0: // waiting to start sending a new BROADCAST packet (sent from the PC) if (UART0_TX_brdcst_first) { // new BROADCAST packet ready to be sent (ReceivePC() controls this variable) last = UART0_TX_brdcst_last; // variable doubles as "ready" flag and pointer to last byte in buffer p = UART0_TX_brdcst_first; UART0_TX_brdcst_last = 0; UART0_TX_brdcst_first = 0; UART0_TX_brdcst_outbuf = 1-UART0_TX_brdcst_outbuf; // switch buffers. here is the only place this switch can be controlled LEDAUX_ON; // turn on AUX LED //UDR0 = *p++; // send first byte out, so that TXC will turn on and state = 1; } else { debug_timer=0; state = 2; // send QUERY packets } return; case 1: // send out the BROADCAST packet UCSR0A|=bit(TXC); // Flag set when last bit has been sent. Must be cleared manually. tested before switching to RX UDR0 = *p++; if (p > last) { // last char in BROADCAST buf has been sent p = 0; last = 0; state = 0; LEDAUX_OFF; } return; case 2: // send QUERY packets (equal to number of servos) if (debug_timer > MotorNextRequestDelay) { servo--; debug_timer=0; } if (!p) { // beginning of sending Query packet if (++servo == Servos_count+1) { // last servo reached => end of queries => restart broadcast packet or more queries // we are in state 2 because state 3 puts us in state 2 and then gets the RX rolling, which then switches back to TX state = 0; servo = 0; return; } LEDAUX_ON; p = UART0_TX_Qbuf; last = UART0_TX_Qbuf_last; // The Query buffer is the same always except for the servo ID and checksum p[7] = ~( UART0_TX_Qbuf_checksum + (p[2] = Servos[servo-1])); } else if (p > last) { // last char in buf has already been sent LEDAUX_OFF; p = 0; last = 0; state = 3; // expect RESPONSE packet return; } UCSR0A|=bit(TXC); // Flag set when last bit has been sent. Must be cleared manually. tested before switching to RX UDR0 = *p++; return; case 3: // wait for RESPONSE packet (following query packet) if (! (UCSR0A&bit(TXC)) ) return; // wait for the last bit of last byte of the query packet to be sent UART0RX; // switch to receive mode UCSR0A|=bit(TXC); // Flag set when last bit has been sent. Must be cleared manually. tested before switching to RX state = 2; return; } } /* ServoReceive - called when expecting packet to be received from servos */ void ServoReceive() { static unsigned char state=0; static unsigned char checksum=0; static unsigned char len=0; static unsigned char *p = (UART1_TX_buf[0]+3); static unsigned short payloadlen=0; static unsigned short UART1_TX_checksum = 0; unsigned char byte = UDR0; if (debug_timer > MotorNextRequestDelay) { state=0; return; } else debug_timer=0; switch (state) { /////// UART0_RX_buf[0]: ff ff id len error parameters checksum case 0: // 0xff if (byte!= 0xff) return;//crash(); state=1; Led_Port &= ~(MANAGELed); return; case 1: // 0xff if (byte != 0xff){ state=0; return;}//crash(); checksum=0; len=0; state=2; return; case 2: // ID checksum=byte; *p++ = byte; UART1_TX_checksum+=byte; state=3; return; case 3: // payload length checksum+=byte; payloadlen=(byte-2); state=4; return; case 4: // error byte if !0 then error found checksum+=byte; //if(byte != 0) crash(); len = 0; state=5; return; case 5: // packet if (payloadlen == len) // servo packet checksum byte { if(( (~checksum)&0xff )!=byte)//crash(); { debug_timer=(MotorNextRequestDelay+1); p = (UART1_TX_buf[UART1_TX_buf_current]+3); Led_Port |= (MANAGELed); state=0; UART0TX; // set UART0 in send mode, next query packet will now be sent} //crash(); return; } state=0; if (servo == Servos_count) // this is the last servo to respond. wrap it up and prepare for next batch. { //servo = 0; *p++ = (~UART1_TX_checksum)&0xff; UART1_TX_checksum = 0; if (!UART1_TX_buf_sending) { UART1_TX_buf_current = (1-UART1_TX_buf_current); p = (UART1_TX_buf[UART1_TX_buf_current]+3); UART1_TX_buf_sending=1; } else { p = (UART1_TX_buf[UART1_TX_buf_current]+3); } Led_Port |= (MANAGELed); } UART0TX; // set UART0 in send mode, next query packet will now be sent } else { *p++ = byte; UART1_TX_checksum+=byte; len++; checksum+=byte; } return; } } // PC sends complete packet to send using SYNC_WRITE // therefore it is up to the PC to figure out handshake packet vs. normal packet(s) /*SIGNAL(USART1_RX_vect)*/ PCReceive(){ // from PC static unsigned char state=0; static unsigned char checksum=0; static unsigned char superpcktcount=0; static unsigned char superpcktlen=0; static unsigned char *p; static unsigned char *first; unsigned char byte = UDR1; Led_Port &= ~(RXLed); switch (state) { // superpacket composed of multiple AX12 broadcast packets: UART0_TX_buf[0]: ff ff fe buflen 83 start count id0 data0 id1 data1... checksum case 0: if(byte==0xFA) state=1; return; case 1: if(byte==0xFA) // found header { state=2; // if a packet was waiting to be sent out to the servos then the incoming packet will overrided it. old packet will be lost lost. UART0_TX_brdcst_last = 0; UART0_TX_brdcst_first = 0; } else // erroneouse bytes { state=0; Led_Port |= (RXLed); } return; case 2: // MSB of the superpacket length superpcktlen = byte; superpcktlen <<= 8; state = 3; //Led_Port |= (RXLed); checksum=byte; return; case 3: // LSB of the superpacket length superpcktlen |= byte; state = 4; first = p = UART0_TX_brdcst[1-UART0_TX_brdcst_outbuf]; superpcktcount = 0; //Led_Port |= (RXLed); checksum+=byte; return; case 4: // payload if (superpcktlen == superpcktcount++) { if(( (~checksum)&0xff )!=byte) { crash(); } // signal to ServoSend() that there's a new broadcast packet ready. UART0_TX_brdcst_first = first; UART0_TX_brdcst_last = p-1; Led_Port |= (RXLed); state=0; } else { *p++ = byte; checksum+=byte; } //Led_Port |= (RXLed); return; } } void InitSerial() { volatile unsigned long u32Temporary=0; // Init UART1 PC UART u32Temporary = (F_CPU/8); u32Temporary = ((u32Temporary*10)/ UART1_Baudrate); if( ((u32Temporary)%10)<5) { u32Temporary=(u32Temporary/10)-1; } else { u32Temporary=(u32Temporary/10); } UBRR1H = ((u32Temporary&0xFF00)>>8); //set baud counter high UBRR1L = (u32Temporary&0xFF); //set baud counter low UCSR1A = bit(U2X); //using double speed UCSR1B = (bit(RXCIE)/*|bit(UDRIE)*/|bit(RXEN)|bit(TXEN)); // enable RX and TX interrupt RX and TX UCSR1B &= ~(bit(RXB8)|bit(TXB8)|bit(UCSZ2)); // disable TX and RX nineth bit and reset UCSZ2, this is used with UCSZ1 and UCSZ0 to determine the length of the word UCSR1C = ~(bit(UMSEL)|bit(UPM0)|bit(UPM1)|bit(USBS)|bit(UCPOL));//disable parity and set assyncronous, set 1stopBit and polarity on rising edge UCSR1C |= (bit(UCSZ1)|bit(UCSZ0)|bit(UCPOL)); // UCSZ2=0 UCSZ1=1 UCSZ0=1 => 8bit length // Init UART0 SERVO UART HalfDuplexTransceiver_Dir |= (HalfDuplexTransceiver_PinOUT|HalfDuplexTransceiver_PinIN); u32Temporary = (F_CPU/8); u32Temporary = ((u32Temporary*10)/UART0_Baudrate); if( ((u32Temporary)%10)<5 ) { u32Temporary=(u32Temporary/10)-1; } else { u32Temporary=(u32Temporary/10); } UBRR0H = ((u32Temporary&0xFF00)>>8); //set baud counter high UBRR0L = (u32Temporary&0xFF); //set baud counter low UCSR0A = bit(U2X); //using double speed UCSR0B = (bit(RXCIE)/*|bit(TXCIE)*/|bit(UDRIE)|bit(RXEN)|bit(TXEN)); // enable RX and TX interrupt RX and TX UCSR0B &= ~(bit(RXB8)|bit(TXB8)|bit(UCSZ2)); // disable TX and RX nineth bit and reset UCSZ2, this is used with UCSZ1 and UCSZ0 to determine the length of the word UCSR0C = ~(bit(UMSEL)|bit(UPM0)|bit(UPM1)|bit(USBS)|bit(UCPOL)); //disable parity and set assyncronous, set 1stopBit and polarity on rising edge UCSR0C |= (bit(UCSZ1)|bit(UCSZ0)|bit(UCPOL)); // UCSZ2=0 UCSZ1=1 UCSZ0=1 => 8bit length //sei(); UCSR0A|=bit(TXC); UCSR1A|=bit(TXC); } void crash(void) { while(1) { Led_Port &= ~(POWERLed|TXLed|RXLed|AUXLed|MANAGELed|PROGRAMLed|PLAYLed); _delay_ms(500); Led_Port |= (POWERLed|TXLed|RXLed|AUXLed|MANAGELed|PROGRAMLed|PLAYLed); _delay_ms(500); } } void SerialCommWrite(uint8_t *pu8DataPointer, uint16_t u16DataLength, uint8_t u8SerialComm) { //volatile uint16_t u16GenericCounter=0; if(u8SerialComm == Servo_Comm) { Led_Port &= ~(AUXLed); UART0TX; while(u16DataLength--) { while((UCSR0A&bit(UDRE))==0){asm("nop");} UCSR0A |= bit(UDRE); UDR0=*pu8DataPointer++; } while((UCSR0A&bit(TXC))==0){asm("nop");} UART0RX; } else { Led_Port &= ~(TXLed); while(u16DataLength--) { while((UCSR1A&bit(UDRE))==0){asm("nop");} UCSR1A |= bit(UDRE); UDR1=*pu8DataPointer++; } while((UCSR1A&bit(TXC))==0){asm("nop");} } Led_Port |= (AUXLed); Led_Port |= (TXLed); } uint8_t SerialCommRead(uint8_t *pu8DataPointer, uint16_t u16DataLength, uint16_t u16TimeoutDelay, uint8_t u8SerialComm) { if(u8SerialComm==Servo_Comm) { while((u16DataLength)&&(u16TimeoutDelay--)) { if(UCSR0A&bit(RXC)) { Led_Port &= ~(AUXLed); (*pu8DataPointer++) = UDR0; u16DataLength--; } } } else if(u8SerialComm == PC_Comm) { while((u16DataLength)&&(u16TimeoutDelay--)) { if(UCSR1A&bit(RXC)) { Led_Port &= ~(RXLed); (*pu8DataPointer++) = UDR1; u16DataLength--; } } } Led_Port |= (AUXLed); Led_Port |= (TXLed); if(u16DataLength==0) return 1; return 0; } uint8_t WriteServo(stServo_Data *pstData) { uint8_t u8TempData[5]={0xFF,0xFF,pstData->u8ServoID,((pstData->u8ServoDataLength)+2),pstData->u8ServoInstruction}; volatile uint32_t u32Sum = ((pstData->u8ServoID)+((pstData->u8ServoDataLength)+2)+(pstData->u8ServoInstruction)); SerialCommWrite(u8TempData, 5, Servo_Comm);//send Header, ID and Instruction SerialCommWrite(pstData->u8ServoData, pstData->u8ServoDataLength, Servo_Comm); //Send Servo Parameters u8TempData[0]=pstData->u8ServoDataLength; while(u8TempData[0]--) { u32Sum+=(pstData->u8ServoData[u8TempData[0]]); } u8TempData[0]=((0xFFFFFFFF-u32Sum)&0xFF); SerialCommWrite(&u8TempData[0], 1, Servo_Comm); return 1; } void PrintDecimal (unsigned long Data) { volatile uint8_t counter=10, firstNum=0, result=0; volatile uint32_t divisor=1000000000; if(Data>0) { while(counter--) { result=Data/divisor; if( (result>0) || (firstNum==1)) { firstNum=1; result+=0x30; SerialCommWrite((uint8_t *)&result,1,PC_Comm); result-=0x30; } Data-=(result*divisor); divisor=(divisor/10); } } else { result=0x30; SerialCommWrite((uint8_t*)&result,1,PC_Comm); } } //#define ENABLE_COMMENTS 1 void find_servos_list(void) { uint8_t u8GenericCounter=1,u8GenericCounter2=0,tryAgain=0, u8TempData[50],u8GenericCounter4=0; stServo_Data pstData,pstData2; uint32_t timeout=0; uint32_t u32Sum=0; #ifdef ENABLE_COMMENTS uint8_t u8GenericCounter3=0; #endif //cli(); //Disable Interrupts pstData.u8ServoID=0; pstData.u8ServoInstruction=AX12_PING; pstData.u8ServoDataLength=0; pstData.u8ServoData[0]=0; // to comment #ifdef ENABLE_COMMENTS SerialCommWrite((uint8_t *)"\n\rSTART\n\r",9,PC_Comm); #endif while((u8GenericCounter2<MAX_SERVOS_ON_BUS) && (u8GenericCounter<MAX_DEVICE_ID)) { pstData.u8ServoID=u8GenericCounter; WriteServo(&pstData); u8TempData[0]=u8TempData[1]=0; timeout=4000; u8GenericCounter4=0; while(timeout--) { if((UCSR0A&bit(RXC))==bit(RXC)) { Led_Port &= ~(AUXLed); //UCSR0A |= bit(RXC); u8TempData[u8GenericCounter4++] = UDR0; } } u32Sum=0; Led_Port |= (AUXLed); if((u8TempData[0]==0xFF) && (u8TempData[1]==0xFF)) { u32Sum=pstData2.u8ServoID = u8TempData[2]; u32Sum+=pstData2.u8ServoInstruction = u8TempData[4];// in this case this will be the error pstData2.u8ServoDataLength = (u8TempData[3]-2); u32Sum+=u8TempData[3]; for(u8GenericCounter4 = 0; u8GenericCounter4 < pstData2.u8ServoDataLength; u8GenericCounter4++ ) { u32Sum += pstData2.u8ServoData[u8GenericCounter4] = u8TempData[5+u8GenericCounter4]; } u8TempData[0]=u8TempData[pstData2.u8ServoDataLength + 5]; // read the checksum to u8TempData[0] u8TempData[1]=((0xFFFFFFFF-u32Sum)&0xFF); //ClearBuffer(Servo_Comm,IN); if(u8TempData[1] == u8TempData[0]) { timeout=0x66;//return 1; } } if(/*ReadServo(&pstData2, 120000)==1*/timeout==0x66) { Servos[u8GenericCounter2]=pstData2.u8ServoID; //to comment #ifdef ENABLE_COMMENTS SerialCommWrite((uint8_t*)"\rFound ID ",11,PC_Comm); PrintDecimal(Servos[u8GenericCounter2]); SerialCommWrite((uint8_t*)"\n\r",2,PC_Comm); #endif tryAgain=0; u8GenericCounter2++; } else { #ifdef ENABLE_COMMENTS if(u8GenericCounter3==0) { SerialCommWrite((uint8_t*)"\r-",2,PC_Comm); u8GenericCounter3++; }else if(u8GenericCounter3==1) { SerialCommWrite((uint8_t*)"\r\\",2,PC_Comm); u8GenericCounter3++; }else if(u8GenericCounter3==2) { SerialCommWrite((uint8_t*)"\r|",2,PC_Comm); u8GenericCounter3++; }else if(u8GenericCounter3==3) { SerialCommWrite((uint8_t*)"\r/",2,PC_Comm); u8GenericCounter3=0; } #endif if(tryAgain==0) { tryAgain=1; } else { tryAgain=0; } } if(tryAgain==0) { u8GenericCounter++; } } Servos_count=u8GenericCounter2; //to comment #ifdef ENABLE_COMMENTS SerialCommWrite((uint8_t*)"\rEND\n\r",7,PC_Comm); SerialCommWrite((uint8_t*)"\rFound ",7,PC_Comm); PrintDecimal(Servos_count); SerialCommWrite((uint8_t*)" servos\n\r",9,PC_Comm); #endif //InitSerial(); UCSR0A|=bit(TXC); UCSR1A|=bit(TXC); //sei();// enable interrupts } int main (void) { unsigned char c; Led_Dir |= (POWERLed|TXLed|RXLed|AUXLed|MANAGELed|PROGRAMLed|PLAYLed); Led_Port |= (POWERLed|TXLed|RXLed|AUXLed|MANAGELed|PROGRAMLed|PLAYLed); cli(); InitSerial(); Led_Port &= ~(POWERLed); find_servos_list(); // sets Servos[] and Servos_count UART1_TX_buf[0][0] = 0xff; UART1_TX_buf[0][1] = 0xff; UART1_TX_buf[0][2] = Servos_count * 3; UART1_TX_buf[1][0] = 0xff; UART1_TX_buf[1][1] = 0xff; UART1_TX_buf[1][2] = Servos_count * 3; // prepare a broadcast packet setting all servos Return Delay to 0 // this will be sent to the servos before starting the query cycles and before PC has a chance to send a broadcast packet UART0_TX_brdcst[0][0] = 0xff; UART0_TX_brdcst[0][1] = 0xff; UART0_TX_brdcst[0][2] = 0xfe; // broadcast UART0_TX_brdcst[0][3] = 0x04; // length UART0_TX_brdcst[0][4] = 0x03; // instruction UART0_TX_brdcst[0][5] = 0x05; // param1 (address) UART0_TX_brdcst[0][6] = 0x01; // param2 (value) UART0_TX_brdcst[0][7] = 0xf4; // ~(fe + 4 + 3 + 5 + 1) & ff UART0_TX_brdcst_last = &UART0_TX_brdcst[0][7]; // pointer to last byte. set to 0 unless new packet just received UART0_TX_brdcst_first = &UART0_TX_brdcst[0][0]; // pointer to first byte. set to 0 unless new packet just received UART0_TX_brdcst_outbuf = 0;// current buffer of the two, being sent //sei(); //wait_for_interrupts(); // just an while(1) -- should use Atmel's sleep options // clear RX0 and RX1 flags by reading UDR0 and UDR1, there shouldnt be anything there. c = UDR0; c = UDR1; UART0TX; // set UART0 to TX mode while(1) { if (debug_timer++ > MotorNextRequestDelay) { ServoReceive(); UART0TX; ServoSend(); } if( UCSR0A&bit(RXC) && UART0_TXRX_state == 1) // receive from servo { ServoReceive(); } if( UCSR0A & bit(UDRE) && UART0_TXRX_state == 0) // UDR register empty. next TX byte can be put on stand-by ServoSend(); if( UCSR1A&bit(RXC) ) // receive from the PC { PCReceive(); } if( (UCSR1A&bit(UDRE)) && UART1_TX_buf_sending) //send to the PC { PCSend(); } } return 1; }
Actuatedcharacter's Blog 
Leave a Reply