Diff: OmniPosition.cpp

Revision:

9:222f7fcbd05a

Parent:

7:73e542a88106

Child:

10:a21aa2bd05c5

Child:

12:edd4217ad7a5

--- a/OmniPosition.cpp	Fri Aug 24 14:48:35 2018 +0900
+++ b/OmniPosition.cpp	Tue Sep 18 15:25:05 2018 +0000
@@ -1,35 +1,23 @@
 #include "OmniPosition.h"
 
 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
-    RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD)
-{
-    attach(callback(this, &OmniPosition::receiveByte));
-    thread.start(callback(this, &OmniPosition::assembleLoop));
-}
-
-void OmniPosition::receiveByte()
+    serial(serialTX, serialRX)
 {
-    buf.push_back(getc());
-}
-
-void OmniPosition::assemble()
-{
-    X = ((buf[2]<<8)|buf[3]) - 32768;
-    Y = ((buf[4]<<8)|buf[5]) - 32768;
-    theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00);
+    thread.start(callback(this, &OmniPosition::assembleLoop));
+    serial.baud(115200);
+    serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
+    serial.startReceive(BUFFER_SIZE);
 }
 
 void OmniPosition::assembleLoop()
 {
     while(true) {
-        if(buf.size() > OP_SERIAL_BUFFER_SIZE) {
-            if(buf[0] == OP_HEADER_FIRST_BYTE && buf[1] == OP_HEADER_SECOND_BYTE) {
-                assemble();
-                buf.erase(buf.begin(), buf.begin() + (OP_SERIAL_BUFFER_SIZE - 1));
-            } else {
-                buf.erase(buf.begin());
-            }
-        }
+        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];
     }
 }
 
@@ -52,7 +40,7 @@
 {
     resetSend = true;
     if(resetSend) {
-        putc('R');
+//        putc('R');
         resetSend = false;
     } else {
         //putc(0);