Revision 13:913b647071a8, committed 2018-10-23

Comitter:

UCHITAKE

Date:

Tue Oct 23 19:29:35 2018 +0900

Parent:

12:edd4217ad7a5

Commit message:

tukaeru

Changed in this revision

--- 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'); }
 

--- a/OmniPosition.h	Fri Oct 12 04:38:04 2018 +0000
+++ b/OmniPosition.h	Tue Oct 23 19:29:35 2018 +0900
@@ -3,38 +3,34 @@
 
 #include "mbed.h"
 
-#include "SerialMultiByte.h"
+#define OP_SERIAL_BUFFER_SIZE 9
+#define OP_HEADER_FIRST_BYTE 72
+#define OP_HEADER_SECOND_BYTE 42
+#define OP_DEFAULT_BAUD 115200
 
-#define FIRST_HEDDER 0xEE
-#define SECOND_HEDDER 0xFF
-#define BUFFER_SIZE 6
-#define TWO_BYTE_DATA 3
-#define ONE_BYTE_DATA 0
+class OmniPosition : public RawSerial {
+ public:
+  OmniPosition(PinName serialTX, PinName serialRX);
 
-class OmniPosition 
-{
-public :
-    OmniPosition(PinName serialTX, PinName serialRX);
+  int16_t getX();
+  int16_t getY();
+  float getTheta();
 
-    int16_t getX();
-    int16_t getY();
-    float getTheta();
+  void reset();
 
-    void reset();
-
-private :
-    void assembleLoop();
+ private:
+  DigitalOut debugled;
+  void receiveByte();
+  void checkData();
 
-    Thread thread;
-
-    int16_t X;
-    int16_t Y;
-    int16_t theta;
-
-    bool resetSend;
-    uint8_t rxdata[BUFFER_SIZE];
-    int data[TWO_BYTE_DATA + ONE_BYTE_DATA];
-    SerialMultiByte serial;
+  uint8_t buffer[9];
+  uint8_t bufferSize;
+  uint8_t bufferPoint;
+  uint8_t receivedBytes;
+  int16_t X;
+  int16_t Y;
+  int16_t theta;
 };
 
-#endif
\ No newline at end of file
+#endif
+