Diff: OmniPosition.cpp

Revision:

12:edd4217ad7a5

Parent:

9:222f7fcbd05a

Child:

13:913b647071a8

--- a/OmniPosition.cpp	Tue Sep 18 15:25:05 2018 +0000
+++ b/OmniPosition.cpp	Fri Oct 12 04:38:04 2018 +0000
@@ -3,7 +3,7 @@
 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
     serial(serialTX, serialRX)
 {
-    thread.start(callback(this, &OmniPosition::assembleLoop));
+//    thread.start(callback(this, &OmniPosition::assembleLoop));
     serial.baud(115200);
     serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
     serial.startReceive(BUFFER_SIZE);
@@ -23,6 +23,12 @@
 
 int16_t OmniPosition::getX()
 {
+    serial.getData(rxdata);
+        for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768;
+        data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00);
+        X = data[0];
+        Y =  data[1];
+        theta = data[2];
     return X;
 }