File content as of revision 6:ce02d396c961:

#include "motor.h"
#include "xyz_sensor_platform.h"
#include "ParseArray.h"


Serial pc(SERIAL_TX, SERIAL_RX);
DigitalOut led(LED2);
XYZSensorPlatform platform;
ParseArray *commandParse;
static const int bufferArrayLength = 100;
static int currentBufferIndex;
static byte* bufferArray;
static byte* dataArray;
byte Xor;
byte commandToSend[10]= {'H','O',0,0,0,0,0,0,'E',0};
static const int dataLength = 7;
void echo(char typ, float x, float y, float z);
void echo(char typ, int16_t *p_data);
int main()
{
    float x, y, z;
    float pos[3];
    led=1;
    bufferArray = new byte[bufferArrayLength];
    dataArray = new byte[dataLength];
    currentBufferIndex = 0;
    commandParse = new ParseArray;
    commandParse->bufferLength = bufferArrayLength;
    commandParse->bufferArray = new byte[bufferArrayLength];
    commandParse->enableHeader(0,1,"48");// 48 H
    commandParse->enableFooter(8,1,"45");// 45 E
    commandParse->enableCheckXOR(9);
    pc.baud(115200);
    platform.set_speed(2.5);
    platform.reset();
    while(1) {
        if(pc.readable()) {
//            commandParse->bufferArray[currentBufferIndex]=pc.getc();
            pc.read((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
//            byte m[]={"read "};
//            while(pc.writeable()!=true);
            pc.write((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
//echo('O',0,0,0);
//pc.printf("...");
            if(commandParse->parse(currentBufferIndex)) {
                pc.write(commandToSend,2,NULL);
                for(int i = commandParse->currentHeaderIndex + 1, j = 0; i < commandParse->currentFooterIndex; i++, j++)
                    dataArray[j] = commandParse->bufferArray[i % commandParse->bufferLength];
                switch(dataArray[0]) {
                    case 'O': // echo
                        platform.position(pos);
                        echo('O',pos[0],pos[1],pos[2]);
                        break;
                    case 'C': // command
                        x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
                        y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
                        z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
                        platform.to(x,y,z);
                        platform.position(pos);
                        echo('O',pos[0],pos[1],pos[2]);
                        break;
                    case 'X':
                        if(dataArray[1]&0x80)platform.go_right();
                        else platform.go_left();
                        platform.position(pos);
                        echo('O',pos[0],pos[1],pos[2]);
                        break;
                    case 'Y':
                        if(dataArray[1]&0x80)platform.go_forward();
                        else platform.go_backward();
                        platform.position(pos);
                        echo('O',pos[0],pos[1],pos[2]);
                        break;
                    case 'Z':
                        if(dataArray[1]&0x80)platform.go_up();
                        else platform.go_down();
                        platform.position(pos);
                        echo('O',pos[0],pos[1],pos[2]);
                        break;
                    case 'M':
                        int16_t mag[3];
                        if(platform.get_mag_raw(mag)==0) echo('M',mag);
                        break;
                    default:
                        break;
                }

            }
            currentBufferIndex++;
            currentBufferIndex%=bufferArrayLength;
        }
        led=!led;
    }

}

void echo(char typ,float x, float y, float z)
{
    byte x_b[2] = {((int)(x*10)>>8)&0xFF, (int)(x*10)&0xFF};
    byte y_b[2] = {((int)(y*10)>>8)&0xFF, (int)(y*10)&0xFF};
    byte z_b[2] = {((int)(z*10)>>8)&0xFF, (int)(z*10)&0xFF};
    
    commandToSend[0] = 'H';
    commandToSend[1] = typ;
    commandToSend[2] = x_b[0];
    commandToSend[3] = x_b[1];
    commandToSend[4] = y_b[0];
    commandToSend[5] = y_b[1];
    commandToSend[6] = z_b[0];
    commandToSend[7] = z_b[1];
    commandToSend[8] = 'E';

    Xor = commandParse->computeXOR(commandToSend,10);
    commandToSend[9]=Xor;
    pc.write(commandToSend,10,NULL);
//    pc.flush();
}

void echo(char typ, int16_t *p_data)
{
    byte tmp[]= {'H', typ, \
                 p_data[0]>>8, p_data[0], \
                 p_data[1]>>8 ,p_data[1], \
                 p_data[2]>>8, p_data[2], \
                 'E',0
                };
    tmp[9]=commandParse->computeXOR(tmp,10);
    pc.printf("%10s",tmp);
}