File content as of revision 12:2e1b1b1726fa:

#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);
InterruptIn button(USER_BUTTON);
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;
bool isEcho = false;
bool isReset = true;
enum PatternRecordState{NONE,INIT,MAGNET,TO};
struct Pattern
{
    float xStart, xEnd, xStep, yStart, yEnd, yStep, zStart, zEnd, zStep;
    int num;
    float x, y, z;
    int count;
    PatternRecordState state;
} pattern;


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();
void released()
{
    led = !led;
    pc.attach(&Rx_interrupt, Serial::RxIrq);
}

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);
    pattern.state = NONE;
    pc.format(8,SerialBase::None,1);
    button.rise(&released);
    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);
    echo('B',0,0,0);
    while(1) {
        if(isReset){
            platform.set_speed(1);
            platform.reset();
            isReset = false;
            platform.set_speed(2.5);
        }
        if(pattern.state != NONE)
        {
            if(pattern.state == INIT) platform.to(pattern.xStart,pattern.yStart,pattern.zStart);
            if(pattern.count == 0) pattern.state = TO;
            if(pattern.state == TO) 
            {
                if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep)) pattern.x += pattern.xStep;
                else if(abs(pattern.y-pattern.yEnd)>abs(pattern.yStep)){
                    pattern.x = pattern.xStart;
                    pattern.y += pattern.yStep;
                }
                else if(abs(pattern.z-pattern.zEnd)>abs(pattern.zStep)){
                    pattern.x = pattern.xStart;
                    pattern.y = pattern.yStart;
                    pattern.z += pattern.zStep;
                }
                else{
                    pattern.state = NONE;
                    continue;
                }
                platform.to(pattern.x,pattern.y,pattern.z);
//                wait(0.);
                isEcho = true;
                pattern.count = pattern.num;
                pattern.state = MAGNET;
            }
            if(pattern.state == MAGNET)
            {
                if(pattern.count-- <= 0) pattern.state = TO;
                getMag++;
            }
        }
        if(isEcho)
        {
            platform.position(pos);
            echo('O',pos[0],pos[1],pos[2]);
            led3 = !led3;
            isEcho = false;
        }
        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': // pattern record
                    if(dataArray[1] == 'X')
                    {
                        pattern.xStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
                        pattern.xEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
                        pattern.xStep = (float) dataArray[6]/10.0f;
                        if((pattern.xStart-pattern.xEnd)*pattern.xStep>0) pattern.xStep = -pattern.xStep;
                    }
                    else if(dataArray[1] == 'Y')
                    {
                        pattern.yStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
                        pattern.yEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
                        pattern.yStep = (float) dataArray[6]/10.0f;
                        if((pattern.yStart-pattern.yEnd)*pattern.yStep>0) pattern.yStep = -pattern.yStep;
                    }
                    else if(dataArray[1] == 'Z')
                    {
                        pattern.zStart = (float)((dataArray[2]<<8)+dataArray[3])/10.0f;
                        pattern.zEnd = (float)((dataArray[4]<<8)+dataArray[5])/10.0f;
                        pattern.zStep = (float) dataArray[6]/10.0f;
                        if((pattern.zStart-pattern.zEnd)*pattern.zStep>0) pattern.zStep = -pattern.zStep;
                    }
                    else if(dataArray[1] == 'N')
                    {
                        pattern.num = (int)dataArray[2];
                    }
                    else if(dataArray[1] == 'M')
                    {
                        if(dataArray[2] == 'B'){
                            pattern.x = (float)((dataArray[3]<<8)+dataArray[4])/10.0f;
                            pattern.y = (float)((dataArray[5]<<8)+dataArray[6])/10.0f;
                        }
                        if(dataArray[2] == 'U'){
                            pattern.z = (float)((dataArray[3]<<8)+dataArray[4])/10.0f;
                            pattern.state = TO;
                            printf("%.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %.2f %.2f %.2f\n %d\n",
                            pattern.xStart, pattern.xEnd, pattern.xStep, pattern.yStart, pattern.yEnd, pattern.yStep, pattern.zStart, pattern.zEnd, pattern.zStep, pattern.x, pattern.y, pattern.z, pattern.num);
                        }
                    }
                        
                    else if(dataArray[1] == 'B')
                    {
                        pattern.state = INIT;
                        pattern.count = pattern.num;
                    }
                    else if(dataArray[1] == 'S') pattern.state = NONE;
                        
                    break;
                case 'O': // echo
                    isEcho = true;
                    break;
                case 'C': // command
                    if(commandToDo) break;
                    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.00052-1/(BAUD/8/10);
                    n = 0;
                    if(waitTime < 0) waitTime = 0;
                    isRecording = true;
                    break;
                case 'S': // stop
                    isRecording = false;
                    break;
                case 'P': // set position
                    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.setPosition(x,y,z);
                    break;
                case 'N': // new set up
                    isReset = true;
                    break;
                default:
                    break;
            } // end switch
            result = NULL;
            dataArray = NULL;
        } // end result if
    } // end parsing if
}