Diff: ui.cpp

Branch:

envelope

Revision:

8:33d34a775873

Parent:

7:ee0569d49c52

Child:

9:c4f7257dee47

--- a/ui.cpp	Fri Dec 08 06:30:58 2017 +0000
+++ b/ui.cpp	Tue Dec 12 08:47:56 2017 +0000
@@ -1,51 +1,61 @@
 #include "motor.h"
 #include "xyz_sensor_platform.h"
-#include "ParseArray.h"
+//#include "ParseArray.h"
+#include "envelopetracker.h"
 
 #define I2C_FREQUENCY 400000
 
-Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+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;
-ParseArray *commandParse;
-static const int bufferArrayLength = 100;
-static int currentBufferIndex;
-static byte* bufferArray;
-static byte* dataArray;
-byte Xor;
+Envelope *command;
+EnvelopeTracker tracker;
 byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
-static const int dataLength = 7;
 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;
-    bufferArray = new byte[bufferArrayLength];
-    dataArray = new byte[dataLength];
-    currentBufferIndex = 0;
-    commandParse = new ParseArray;
-    commandParse->bufferLength = bufferArrayLength;
-    commandParse->bufferArray = new byte[bufferArrayLength];
-    commandParse->enableHeader(0,1,"48");// 48 H
-    commandParse->enableFooter(8,1,"45");// 45 E
-    commandParse->enableCheckXOR(9);
-    commandParse->length=10;
+    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()) {
-            pc.read((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
-            if(commandParse->parse(currentBufferIndex)) {
-                for(int i = commandParse->currentHeaderIndex + 1, j = 0; i < commandParse->currentFooterIndex; i++, j++)
-                    dataArray[j] = commandParse->bufferArray[i % commandParse->bufferLength];
+            c = pc.getc();
+            tracker.parse(&c,1);
+
+            result = tracker.getEnvelope();
+            if(result!=NULL) {
+                char *dataArray = result->getPayload();
                 switch(dataArray[0]) {
                     case 'I':
                         break;
@@ -86,6 +96,10 @@
                         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
@@ -93,21 +107,28 @@
                         break;
                     default:
                         break;
-                }
-
-            }
-            currentBufferIndex++;
-            currentBufferIndex%=bufferArrayLength;
+                } // end switch
+//                delete result;
+                delete dataArray;
+                result = NULL;
+                dataArray = NULL;
+            } // end result if
         } // end parsing if
-        if(isRecording && n < 10000) {
+        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(0.0001);
+                wait(waitTime);
             }
             n++;
-        } // end recording if
+        } 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
 
 }
@@ -120,10 +141,26 @@
 
 void echo(char typ, int16_t *p_data)
 {
-    byte tmp[10]= {'H', typ, p_data[0]>>8, p_data[0],  p_data[1]>>8 ,p_data[1],  p_data[2]>>8, p_data[2],  45,'\0' };
-    tmp[8]='E';
-    tmp[9]=commandParse->computeXOR(tmp,9);
-    pc.write(tmp,10,NULL);
+//    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;
 }