Diff: ui.cpp

Revision:

6:ce02d396c961

Child:

7:ee0569d49c52

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ui.cpp	Fri Nov 10 02:39:32 2017 +0000
@@ -0,0 +1,131 @@
+#include "motor.h"
+#include "xyz_sensor_platform.h"
+#include "ParseArray.h"
+
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+DigitalOut led(LED2);
+XYZSensorPlatform platform;
+ParseArray *commandParse;
+static const int bufferArrayLength = 100;
+static int currentBufferIndex;
+static byte* bufferArray;
+static byte* dataArray;
+byte Xor;
+byte commandToSend[10]= {'H','O',0,0,0,0,0,0,'E',0};
+static const int dataLength = 7;
+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];
+    led=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);
+    pc.baud(115200);
+    platform.set_speed(2.5);
+    platform.reset();
+    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 'O': // echo
+                        platform.position(pos);
+                        echo('O',pos[0],pos[1],pos[2]);
+                        break;
+                    case 'C': // command
+                        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.to(x,y,z);
+                        platform.position(pos);
+                        echo('O',pos[0],pos[1],pos[2]);
+                        break;
+                    case 'X':
+                        if(dataArray[1]&0x80)platform.go_right();
+                        else platform.go_left();
+                        platform.position(pos);
+                        echo('O',pos[0],pos[1],pos[2]);
+                        break;
+                    case 'Y':
+                        if(dataArray[1]&0x80)platform.go_forward();
+                        else platform.go_backward();
+                        platform.position(pos);
+                        echo('O',pos[0],pos[1],pos[2]);
+                        break;
+                    case 'Z':
+                        if(dataArray[1]&0x80)platform.go_up();
+                        else platform.go_down();
+                        platform.position(pos);
+                        echo('O',pos[0],pos[1],pos[2]);
+                        break;
+                    case 'M':
+                        int16_t mag[3];
+                        if(platform.get_mag_raw(mag)==0) echo('M',mag);
+                        break;
+                    default:
+                        break;
+                }
+
+            }
+            currentBufferIndex++;
+            currentBufferIndex%=bufferArrayLength;
+        }
+        led=!led;
+    }
+
+}
+
+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();
+}
+
+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);
+}
+
+