Diff: ui.cpp

Revision:

0:9e6e3dc903c0

Child:

1:edc6b5cc7112

diff -r 000000000000 -r 9e6e3dc903c0 ui.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ui.cpp	Sat Dec 22 01:07:34 2018 +0000
@@ -0,0 +1,394 @@
+#include "xyz_sensor_platform.h"
+#include "envelopetracker.h"
+
+#define SPI_FREQUENCY 1e5 
+#define BIG_CHAR_MASK 0x1F
+#define BAUD 921600
+#define Fs 1e2 // 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;
+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);
+}
+
+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);
+    platform.set_speed(2.5);
+    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( getMag == 0 ){
+                    if(pattern.count-- <= 0) pattern.state = TO;
+                    getMag++;
+                    nextRecord.attach(&recordTimeup, 1.0f/(float)Fs);
+                }
+            }
+        }
+        if(isEcho)
+        {
+            platform.position(pos);
+            echo('O',pos[0],pos[1],pos[2]);
+            led3 = !led3;
+            isEcho = false;
+        }
+        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]);
+            }
+            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]);
+            }
+            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]);
+    }
+    return dataToSend;
+}
+
+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;
+                    if( recordTime == 0 ) recordTime = dataArray[3]*Fs/100;
+                    magSel = dataArray[2];
+                    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
+}