File content as of revision 8:33d34a775873:

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

#define I2C_FREQUENCY 400000

typedef unsigned char byte;
uint8_t** dataToSend;
int sendArrayIndex = 0;
int sendBufferMax = 10000;
const int BAUD = 921600;
Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
DigitalOut led(LED2),led3(LED3);
DigitalOut mag_test(D11);
XYZSensorPlatform platform;
Envelope *command;
EnvelopeTracker tracker;
byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
bool isRecording = false;

const int Fs = 100; // sampling rate -- max: 1kHz


void echo(char typ, float x, float y, float z);
void echo(char typ, int16_t *p_data);

int main()
{
    Envelope* result;
    float x, y, z;
    float pos[3];
    int n = 0;
    int recordTime;
    float waitTime;
    dataToSend = new uint8_t*[sendBufferMax];
//    uint8_t *ch_in = new char [10];
    led=1;
    mag_test=1;
    command = new Envelope;
    command->enableHeader(std::string("H"));// 48 H
    command->enableFooter(std::string("E"),8);// 45 E
    command->enableCheckXOR(9);
    tracker.setEnvelope(*command);
    tracker.setBufferLength(100);
    pc.format(8,SerialBase::None,1);
    platform.set_speed(2.5);
    platform.reset();       // need to be modified here
    platform.setSensorI2cFrequency(I2C_FREQUENCY);
    char c;
    while(1) {
        if(pc.readable()) {
            c = pc.getc();
            tracker.parse(&c,1);

            result = tracker.getEnvelope();
            if(result!=NULL) {
                char *dataArray = result->getPayload();
                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
                        recordTime = dataArray[1];
                        recordTime *= Fs;
                        waitTime = 1.0/Fs-0.0005-1/(BAUD/8/10);
                        if(waitTime < 0) waitTime = 0;
                        isRecording = true;
                        break;
                    case 'S': // stop
                        isRecording = false;
                        break;
                    default:
                        break;
                } // end switch
//                delete result;
                delete dataArray;
                result = NULL;
                dataArray = NULL;
            } // end result if
        } // end parsing if
        if(isRecording && n < recordTime) {
            int16_t mag[3];
            if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
                echo('M',mag);
                mag_test=!mag_test;
                wait(waitTime);
            }
            n++;
        } else if(isRecording) {
            n = 0;
            isRecording = false;
            for(int i = 0; i < sendBufferMax; i++)
            if(dataToSend[i]) delete dataToSend[i];
            echo('S',0,0,0);
        }// 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)
{
//    char * tmp = new char [7];
//    tmp[0] = typ;
//    tmp[1] = p_data[0]>>8;
//    tmp[2] = p_data[0];
//    tmp[3] = p_data[1]>>8;
//    tmp[4] = p_data[1];
//    tmp[5] = p_data[2]>>8;
//    tmp[6] = p_data[2];
    char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
    command->setEnvelopeData(tmp,7);
//    if(dataToSend[sendArrayIndex]) delete dataToSend[sendArrayIndex];
    dataToSend[sendArrayIndex] = (uint8_t*)(command->getEnvelopeArray());
    for(int i = 0; i < command->length(); i++) {
//    pc.write(dataToSend[sendArrayIndex],command->length(),NULL);
//        wait(0.0002);
        pc.putc(dataToSend[sendArrayIndex][i]);
    }
    sendArrayIndex = (sendArrayIndex+1)%sendBufferMax;
//    delete dataToSend;
//    dataToSend = NULL;
}