File content as of revision 10:a49cdab3810f:

#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;
Envelope* result;
float x, y, z;
float pos[3];
int n = 0;
int getMag = 0;
int leftCount = 0;
int rightCount = 0;
int upCount = 0;
int downCount = 0;
int forwardCount = 0;
int backwardCount = 0;
bool commandToDo = false;
int recordTime;
float waitTime;

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


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

int main()
{
    led=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);
// Setup a serial interrupt function to receive data
    pc.attach(&Rx_interrupt, Serial::RxIrq);
    while(1) {
        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) {
            isRecording = false;
            echo('S',0,0,0);
        }// end recording if
        if(getMag>0) {
            int16_t mag[3];
            if(platform.get_mag_raw(mag)==0) echo('M',mag);
            getMag--;
        }
        if(commandToDo) {
            platform.to(x,y,z);
            platform.position(pos);
            echo('O',pos[0],pos[1],pos[2]);
            commandToDo = false;
        }
        if(leftCount > 0||rightCount > 0||upCount > 0||downCount > 0||forwardCount > 0||backwardCount > 0) {
            if(leftCount > 0) {
                platform.go_left();
                leftCount--;
            }
            if(rightCount > 0) {
                platform.go_right();
                rightCount--;
            }
            if(upCount > 0) {
                platform.go_up();
                upCount--;
            }
            if(downCount > 0) {
                platform.go_down();
                downCount--;
            }
            if(forwardCount > 0) {
                platform.go_forward();
                forwardCount--;
            }
            if(backwardCount > 0) {
                platform.go_backward();
                backwardCount--;
            }
            platform.position(pos);
            echo('O',pos[0],pos[1],pos[2]);
        }
    } // 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[] = {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);
    dataToSend = (uint8_t*)(command->getEnvelopeArray());
    for(int i = 0; i < command->length(); i++) {
        pc.putc(dataToSend[i]);
    }
}

void Rx_interrupt()
{
    char c;
    while(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;
                    commandToDo = true;
                    break;
                case 'X':
                    if(dataArray[1]&0x80) rightCount++;
                    else leftCount++;
                    break;
                case 'Y':
                    if(dataArray[1]&0x80) forwardCount++;
                    else backwardCount++;
                    break;
                case 'Z':
                    if(dataArray[1]&0x80) upCount++;
                    else downCount++;
                    break;
                case 'M': // magnet
                    getMag++;
                    mag_test=!mag_test;
                    break;
                case 'R': // record
                    recordTime = dataArray[1];
                    recordTime *= Fs;
                    waitTime = 1.0/Fs-0.00048-1/(BAUD/8/10);
                    n = 0;
                    if(waitTime < 0) waitTime = 0;
                    isRecording = true;
                    break;
                case 'S': // stop
                    isRecording = false;
                    break;
                default:
                    break;
            } // end switch
//            delete dataArray;
            result = NULL;
            dataArray = NULL;
        } // end result if
    } // end parsing if
}