Diff: ui.cpp

Revision:

7:ee0569d49c52

Parent:

6:ce02d396c961

Child:

8:33d34a775873

--- a/ui.cpp	Fri Nov 10 02:39:32 2017 +0000
+++ b/ui.cpp	Fri Dec 08 06:30:58 2017 +0000
@@ -2,9 +2,11 @@
 #include "xyz_sensor_platform.h"
 #include "ParseArray.h"
 
+#define I2C_FREQUENCY 400000
 
-Serial pc(SERIAL_TX, SERIAL_RX);
-DigitalOut led(LED2);
+Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+DigitalOut led(LED2),led3(LED3);
+DigitalOut mag_test(D11);
 XYZSensorPlatform platform;
 ParseArray *commandParse;
 static const int bufferArrayLength = 100;
@@ -12,15 +14,18 @@
 static byte* bufferArray;
 static byte* dataArray;
 byte Xor;
-byte commandToSend[10]= {'H','O',0,0,0,0,0,0,'E',0};
+byte commandToSend[10]= {'H','O','1','2','3','4','5','6','E',0};
 static const int dataLength = 7;
+bool isRecording = false;
 void echo(char typ, float x, float y, float z);
 void echo(char typ, int16_t *p_data);
 int main()
 {
     float x, y, z;
     float pos[3];
+    int n = 0;
     led=1;
+    mag_test=1;
     bufferArray = new byte[bufferArrayLength];
     dataArray = new byte[dataLength];
     currentBufferIndex = 0;
@@ -30,26 +35,24 @@
     commandParse->enableHeader(0,1,"48");// 48 H
     commandParse->enableFooter(8,1,"45");// 45 E
     commandParse->enableCheckXOR(9);
-    pc.baud(115200);
+    commandParse->length=10;
+    pc.format(8,SerialBase::None,1);
     platform.set_speed(2.5);
-    platform.reset();
+    platform.reset();       // need to be modified here
+    platform.setSensorI2cFrequency(I2C_FREQUENCY);
     while(1) {
         if(pc.readable()) {
-//            commandParse->bufferArray[currentBufferIndex]=pc.getc();
             pc.read((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
-//            byte m[]={"read "};
-//            while(pc.writeable()!=true);
-            pc.write((uint8_t*)&(commandParse->bufferArray[currentBufferIndex]),1,NULL);
-//echo('O',0,0,0);
-//pc.printf("...");
             if(commandParse->parse(currentBufferIndex)) {
-                pc.write(commandToSend,2,NULL);
                 for(int i = commandParse->currentHeaderIndex + 1, j = 0; i < commandParse->currentFooterIndex; i++, j++)
                     dataArray[j] = commandParse->bufferArray[i % commandParse->bufferLength];
                 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;
@@ -77,9 +80,16 @@
                         platform.position(pos);
                         echo('O',pos[0],pos[1],pos[2]);
                         break;
-                    case 'M':
+                    case 'M': // magnet
                         int16_t mag[3];
                         if(platform.get_mag_raw(mag)==0) echo('M',mag);
+                        mag_test=!mag_test;
+                        break;
+                    case 'R': // record
+                        isRecording = true;
+                        break;
+                    case 'S': // stop
+                        isRecording = false;
                         break;
                     default:
                         break;
@@ -88,44 +98,32 @@
             }
             currentBufferIndex++;
             currentBufferIndex%=bufferArrayLength;
-        }
-        led=!led;
-    }
+        } // end parsing if
+        if(isRecording && n < 10000) {
+            int16_t mag[3];
+            if(platform.get_mag_raw(mag)==0&&pc.writeable()) {
+                echo('M',mag);
+                mag_test=!mag_test;
+                wait(0.0001);
+            }
+            n++;
+        } // end recording if
+    } // end while
 
 }
 
 void echo(char typ,float x, float y, float z)
 {
-    byte x_b[2] = {((int)(x*10)>>8)&0xFF, (int)(x*10)&0xFF};
-    byte y_b[2] = {((int)(y*10)>>8)&0xFF, (int)(y*10)&0xFF};
-    byte z_b[2] = {((int)(z*10)>>8)&0xFF, (int)(z*10)&0xFF};
-    
-    commandToSend[0] = 'H';
-    commandToSend[1] = typ;
-    commandToSend[2] = x_b[0];
-    commandToSend[3] = x_b[1];
-    commandToSend[4] = y_b[0];
-    commandToSend[5] = y_b[1];
-    commandToSend[6] = z_b[0];
-    commandToSend[7] = z_b[1];
-    commandToSend[8] = 'E';
-
-    Xor = commandParse->computeXOR(commandToSend,10);
-    commandToSend[9]=Xor;
-    pc.write(commandToSend,10,NULL);
-//    pc.flush();
+    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)
 {
-    byte tmp[]= {'H', typ, \
-                 p_data[0]>>8, p_data[0], \
-                 p_data[1]>>8 ,p_data[1], \
-                 p_data[2]>>8, p_data[2], \
-                 'E',0
-                };
-    tmp[9]=commandParse->computeXOR(tmp,10);
-    pc.printf("%10s",tmp);
+    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);
 }