CM5 code to interface Roboard and Servos

by

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

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s


%d bloggers like this: