Diff: OmniPosition.cpp

Revision:

6:f8dbbe93bc7b

Parent:

5:f8c3aeb4e65f

Child:

7:73e542a88106

--- a/OmniPosition.cpp	Wed Aug 22 20:21:48 2018 +0900
+++ b/OmniPosition.cpp	Fri Aug 24 14:40:25 2018 +0900
@@ -1,103 +1,37 @@
 #include "OmniPosition.h"
 
 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
-    RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD),
-    readCounter(0),
-    takeCounter(0),
-    X(0),
-    Y(0),
-    theta(0.0)
+    RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD)
 {
-    buffer = new char[OP_SERIAL_BUFFER_SIZE];
-    data = new char[2];
-    attach(callback(this, &OmniPosition::readData));
-    assembleTicker.attach(callback(this, &OmniPosition::assemble), OP_RECEIVE_FREQ);
+    attach(callback(this, &OmniPosition::receiveByte));
+    thread.start(callback(this, &OmniPosition::assembleLoop));
     sendTicker.attach(callback(this, &OmniPosition::send), OP_SEND_FREQ);
 }
 
-void OmniPosition::readData()
+void OmniPosition::receiveByte()
 {
-    buffer[(int)readCounter] = getc();
-    readCounter = incrementCounter(readCounter);
+    buf.push_back(getc());
 }
 
 void OmniPosition::assemble()
 {
-    //Find header
-    headerCheck = false;
-    headerPoint = 0xff;
+    X = ((buf[2]<<8)|buf[3]) - 32768;
+    Y = ((buf[4]<<8)|buf[5]) - 32768;
+    theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00);
+}
 
-    for(int i = 0; i < OP_SERIAL_BUFFER_SIZE; i++) {
-        if(buffer[i] == OP_HEADER_FIRST_BYTE) {
-            takeCounter = i;
-            takeCounter = incrementCounter(takeCounter);
-            if(buffer[(int)takeCounter] == OP_HEADER_SECOND_BYTE) {
-                headerCheck = true;
-                headerPoint = i;
+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());
             }
         }
     }
-    if(headerPoint == 0xff) {
-        return;
-    }
-
-    //assemble
-    checksum = 0;
-    takeCounter = headerPoint;  //firstheader
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    data[0] = buffer[(int)takeCounter];
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    data[1] = buffer[(int)takeCounter];
-    X = ((data[0]<<8)|data[1]) - 32768;
-    checksum = (data[0] ^ data[1]);
-
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    data[0] = buffer[(int)takeCounter];
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    data[1] = buffer[(int)takeCounter];
-    Y = ((data[0]<<8)|data[1]) - 32768;
-    checksum = (checksum ^ data[0]);
-    checksum = (checksum ^ data[1]);
-
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    data[0] = buffer[(int)takeCounter];
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    data[1] = buffer[(int)takeCounter];
-    thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
-    theta = thetaint / 100.0;
-    if(theta > 180.0) {
-        theta -= 655.0;
-    }
-    theta *= -M_PI / 180.0;
-    if(theta > M_PI) theta -= 2 * M_PI;
-    if(theta < -M_PI) theta += 2 * M_PI;
-    if(theta > M_PI) theta -= 2 * M_PI;
-    if(theta < -M_PI) theta += 2 * M_PI;
-    checksum = (checksum ^ data[0]);
-    checksum = (checksum ^ data[1]);
-
-    takeCounter = incrementCounter(takeCounter);  //secondheader
-    if(buffer[(int)takeCounter] != checksum) {
-        X = bfrX;
-        Y = bfrY;
-        theta = bfrTheta;
-    } else {
-        bfrX = X;
-        bfrY = Y;
-        bfrTheta = theta;
-    }
-    
-}
-
-int OmniPosition::incrementCounter(int counter)
-{
-    ++counter;
-    if(counter >= OP_SERIAL_BUFFER_SIZE) {
-        counter -= OP_SERIAL_BUFFER_SIZE;
-    }
-    return counter;
 }
 
 void OmniPosition::send()
@@ -110,19 +44,19 @@
     }
 }
 
-int OmniPosition::getX()
+int16_t OmniPosition::getX()
 {
     return X;
 }
 
-int OmniPosition::getY()
+int16_t OmniPosition::getY()
 {
     return Y;
 }
 
-double OmniPosition::getTheta()
+float OmniPosition::getTheta()
 {
-    return -theta;
+    return (float)(theta / 100.0);
 }
 
 void OmniPosition::reset()