File content as of revision 7:ee0569d49c52:

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

#define I2C_FREQUENCY 400000

Serial pc(SERIAL_TX, SERIAL_RX, 115200);
DigitalOut led(LED2),led3(LED3);
DigitalOut mag_test(D11);
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','1','2','3','4','5','6','E',0};
static const int dataLength = 7;
bool isRecording = false;
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];
    int n = 0;
    led=1;
    mag_test=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);
    commandParse->length=10;
    pc.format(8,SerialBase::None,1);
    platform.set_speed(2.5);
    platform.reset();       // need to be modified here
    platform.setSensorI2cFrequency(I2C_FREQUENCY);
    while(1) {
        if(pc.readable()) {
            pc.read((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
            if(commandParse->parse(currentBufferIndex)) {
                for(int i = commandParse->currentHeaderIndex + 1, j = 0; i < commandParse->currentFooterIndex; i++, j++)
                    dataArray[j] = commandParse->bufferArray[i % commandParse->bufferLength];
                switch(dataArray[0]) {
                    case 'I':
                        break;
                    case 'O': // echo
                        platform.position(pos);
                        echo('O',pos[0],pos[1],pos[2]);
                        led3 = !led3;
                        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': // magnet
                        int16_t mag[3];
                        if(platform.get_mag_raw(mag)==0) echo('M',mag);
                        mag_test=!mag_test;
                        break;
                    case 'R': // record
                        isRecording = true;
                        break;
                    case 'S': // stop
                        isRecording = false;
                        break;
                    default:
                        break;
                }

            }
            currentBufferIndex++;
            currentBufferIndex%=bufferArrayLength;
        } // end parsing if
        if(isRecording && n < 10000) {
            int16_t mag[3];
            if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
                echo('M',mag);
                mag_test=!mag_test;
                wait(0.0001);
            }
            n++;
        } // end recording if
    } // end while

}

void echo(char typ,float x, float y, float z)
{
    int16_t p_data[3]= {(int16_t)(x*10), int16_t(y*10), int16_t(z*10)};
    echo(typ,p_data);
}

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