File content as of revision 15:67c0fbc0999a:

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

#define I2C_FREQUENCY 400000
#define SPI_FREQUENCY 0.1e7
//#define DELAY_COMPENSATE 0.000537
#define BIG_CHAR_MASK 0x1F
#define BAUD 921600
#define Fs 1e3 // sampling rate -- max: 1kHz

typedef unsigned char byte;
uint8_t* dataToSend;
int sendArrayIndex = 0;
int sendBufferMax = 10000;
Serial pc(SERIAL_TX, SERIAL_RX, BAUD );
DigitalOut led(LED2),led3(LED3);
DigitalOut mag_test(PC_8);
InterruptIn button(USER_BUTTON);
Timeout nextRecord;
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;
char magSel = 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 = false;
bool isTimeToRecord = false;
bool isMagReset = false;
uint8_t magData[80] = {0};  //80 bytes
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;



void echo(char typ, float x, float y, float z);
uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow = true);
void Rx_interrupt();
void recordTimeup()
{
    isTimeToRecord = true;
}
void released()
{
    led = !led;
    pc.attach(&Rx_interrupt, Serial::RxIrq);
}
void no_call(){}

int main()
{
    led=1;
//    char msg[] = "start!!";
//    fwrite(msg,1, 7,stdout);
    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.setSensorSpiFrequency(SPI_FREQUENCY);
// Setup a serial interrupt function to receive data
    pc.attach(&Rx_interrupt, Serial::RxIrq);
//    echo('B',0,0,0);
    while(1) {
        if(platform.isMoving()) continue;
        if(isReset){
            platform.set_speed(1);
            platform.reset();
            isReset = false;
            platform.set_speed(2.5);
        }
        if(pattern.state != NONE)
        {
            if(pattern.state == INIT)
            {
                pattern.x = pattern.xStart;
                pattern.y = pattern.yStart;
                pattern.z = pattern.zStart;
                pattern.count = pattern.num;
                pattern.state = MAGNET;
                //isEcho = true;
                platform.to(pattern.x,pattern.y,pattern.z);
                isEcho = true;
                continue;
            }
            if(pattern.count == 0) pattern.state = TO;
            if(pattern.state == TO) 
            {
                if(abs(pattern.x-pattern.xEnd)>abs(pattern.xStep))
                {
                    pattern.x += pattern.xStep;
                    platform.set_speed(1.5);
                }
                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;
                    echo('S',0,0,0);
                    continue;
                }
                platform.to(pattern.x,pattern.y,pattern.z);
                isEcho = true;
                pattern.count = pattern.num;
                pattern.state = MAGNET;
                platform.set_speed(2.5);
                continue;
            }
            if(pattern.state == MAGNET)
            {
                if(pattern.count-- <= 0) pattern.state = TO;
                getMag++;
                nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
//                wait(waitTime);
            }
        }
        if(isEcho)
        {
            platform.position(pos);
            echo('O',pos[0],pos[1],pos[2]);
            led3 = !led3;
            isEcho = false;
            //    printf("  %f  ",1.0f/(float)Fs);
        }
        if(isRecording && n < recordTime && isTimeToRecord) {
            nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
            isTimeToRecord = false;
            char curMag = 1;
            int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0};
            mag_test=!mag_test;
            for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
            {
                if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0]));
                curMag <<= 1;
            }
            curMag = 1;
            int line = 0;
            for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
            {
                if((curMag & magSel) != 0) 
                {
                    memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10);
                }
                curMag <<= 1;
            }
            if(pc.writeable()) {
                for(int i = 0; i < line*10; i++) pc.putc(magData[i]);
                //pc.putc('\0');
            }/*
            if(pc.writeable()) {
                for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
                {
                    if((curMag & magSel) != 0) echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0]);
                    curMag <<= 1;
                }
//                wait(waitTime);
            }*/
            n++;
            if(isRecording && n == recordTime) {
                nextRecord.detach();
                n = 0;
                isRecording = false;
                echo('S',0,0,0);
            }
        }// end recording if
        if(getMag>0 && isTimeToRecord) {
            isTimeToRecord = false;
            char curMag = 1;
            int16_t mag[NUMBER_OF_MAGNETOMETERS][3] = {0};
            for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
            {
                if((curMag & magSel) != 0) platform.get_mag_raw(i, &(mag[i][0]));
                curMag <<= 1;
            }
            curMag = 1;
            int line = 0;
            for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
            {
                if((curMag & magSel) != 0) 
                {
                    memcpy(&magData[line++*10],echo(('M'&BIG_CHAR_MASK)|(i<<5),&mag[i][0],false),10);
                }
                curMag <<= 1;
            }
            if(pc.writeable()) {
                for(int i = 0; i < line*10; i++) pc.putc(magData[i]);
                //pc.putc('\0');
            }
            getMag--;
        }
        if(commandToDo) {
            platform.to(x,y,z);
            isEcho = true;
            commandToDo = false;
            continue;
        }
        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--;
            }
            isEcho = true;
        }
        if(isMagReset)
        {
            isMagReset = false;
            char curMag = 1;
            for(int i = 0; i < NUMBER_OF_MAGNETOMETERS; i++)
            {
                if((curMag & magSel) != 0) platform.resetMagnetometer(i);
                curMag <<= 1;
            }
        }
    } // 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);
}

uint8_t* echo(char typ, int16_t *p_data, bool isWriteNow)
{
    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());
    if(pc.writeable()&&isWriteNow)
    {
       for(int i = 0; i < 10; i++) pc.putc(dataToSend[i]);
       // pc.printf("%.*s",10,dataToSend);
        //pc.fsync();
        //pc.putc('\0');
    }
    return dataToSend;
    /*
    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]) {
                // following alphabets is used
                // I O C X Y Z M R S P N B
                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]<<8)+dataArray[3]);
                        isTimeToRecord = true;
//                        waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
                    }
                    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') // initial
                    {
                        pattern.state = INIT;
                    }
                    else if(dataArray[1] == 'S') pattern.state = NONE;// stop
                        
                    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': // move in X direction
                    if(dataArray[1]&0x80) rightCount++;
                    else leftCount++;
                    break;
                case 'Y': // move in Y direction
                    if(dataArray[1]&0x80) forwardCount++;
                    else backwardCount++;
                    break;
                case 'Z': // move in Z direction
                    if(dataArray[1]&0x80) upCount++;
                    else downCount++;
                    break;
                case 'M': // magnet
                    getMag++;
                    magSel = dataArray[1];
                    isTimeToRecord = true;
                    pc.putc('M');
                    break;
                case 'R': // record
                    recordTime = dataArray[1];
                    recordTime *= Fs;
                    magSel = dataArray[2];
//                    mag_test=!mag_test;
                    /*
                    waitTime = 1.0/Fs-DELAY_COMPENSATE-1/(BAUD/8/10);
                    n = 0;
                    if(waitTime < 0) waitTime = 0;*/
                    isTimeToRecord = true;
                    isRecording = true;
                    break;
                case 'S': // stop
                    isRecording = false;
                    nextRecord.detach();
                    isTimeToRecord = 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;
                case 'B':
                    magSel = dataArray[1];
                    isMagReset = true;
                    break;
                default:
                    break;
            } // end switch
            result = NULL;
            dataArray = NULL;
        } // end result if
    } // end parsing if
}