Diff: OmniPosition.cpp

Revision:

13:913b647071a8

Parent:

12:edd4217ad7a5

--- a/OmniPosition.cpp	Fri Oct 12 04:38:04 2018 +0000
+++ b/OmniPosition.cpp	Tue Oct 23 19:29:35 2018 +0900
@@ -1,55 +1,62 @@
 #include "OmniPosition.h"
 
-OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
-    serial(serialTX, serialRX)
-{
-//    thread.start(callback(this, &OmniPosition::assembleLoop));
-    serial.baud(115200);
-    serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
-    serial.startReceive(BUFFER_SIZE);
+OmniPosition::OmniPosition(PinName serialTX, PinName serialRX)
+    : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PA_11) {
+  attach(callback(this, &OmniPosition::receiveByte));
+  X = 0;
+  Y = 0;
+  bufferSize = 9;
+  bufferPoint = 0;
+  receivedBytes = 0;
 }
 
-void OmniPosition::assembleLoop()
-{
-    while(true) {
-        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];
-    }
+void OmniPosition::receiveByte() {
+  buffer[bufferPoint % bufferSize] = getc();
+  if (bufferPoint != 0xff) {
+    ++bufferPoint;
+  } else {
+    bufferPoint = (255 % bufferSize) + 1;
+  }
+
+  ++receivedBytes;
+
+  if (receivedBytes >= bufferSize) {
+    checkData();
+  }
 }
 
-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;
-}
-
-int16_t OmniPosition::getY()
-{
-    return Y;
+void OmniPosition::checkData() {
+  for (int i = 0; i < bufferSize; i++) {
+    if (buffer[i % bufferSize] == OP_HEADER_FIRST_BYTE &&
+        buffer[(i + 1) % bufferSize] == OP_HEADER_SECOND_BYTE) {
+      uint8_t checksum = 0;
+      for (int j = 0; j < bufferSize - 3; j++) {
+        checksum += buffer[(i + 2 + j) % bufferSize];
+      }
+      if ((uint8_t)checksum == buffer[(i + bufferSize - 1) % bufferSize]) {
+        debugled = !debugled;
+        X = ((buffer[(i + 2 + 0) % bufferSize] << 8) |
+             buffer[(i + 2 + 1) % bufferSize]) -
+            32768;
+        Y = ((buffer[(i + 2 + 2) % bufferSize] << 8) |
+             buffer[(i + 2 + 3) % bufferSize]) -
+            32768;
+        theta = (buffer[(i + 2 + 4) % bufferSize] & 0xFF) |
+                ((buffer[(i + 2 + 5) % bufferSize] << 8) & 0xFF00);
+        receivedBytes = 0;
+        return;
+      }
+    }
+  }
 }
 
-float OmniPosition::getTheta()
-{
-    return (float)(theta / 100.0);
+int16_t OmniPosition::getX() { return X; }
+
+int16_t OmniPosition::getY() { return Y; }
+
+float OmniPosition::getTheta() {
+  return (float)(theta / 100.0) * (M_PI / 180.0);
 }
 
-void OmniPosition::reset()
-{
-    resetSend = true;
-    if(resetSend) {
-//        putc('R');
-        resetSend = false;
-    } else {
-        //putc(0);
-    }
-}
+void OmniPosition::reset() { putc('R'); }