library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

Revision:
2:edd33d3ad0fd
Parent:
1:0229fc98a26f
Child:
3:47676abdf529
--- a/OmniPosition.cpp	Sat Jun 30 14:23:07 2018 +0900
+++ b/OmniPosition.cpp	Sun Jul 01 16:39:13 2018 +0900
@@ -3,11 +3,15 @@
 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
     RawSerial(serialTX, serialRX, DEFAULT_BAUD),
     readCounter(SERIAL_BUFFER_SIZE, 0),
-    takeCounter(SERIAL_BUFFER_SIZE, 0)
+    takeCounter(SERIAL_BUFFER_SIZE, 0),
+    X(0),
+    Y(0)
 {
     buffer = new char[SERIAL_BUFFER_SIZE];
+    data = new char[2];
     attach(callback(this, &OmniPosition::readData));
-    ticker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
+    assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
+    sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ);
 }
 
 void OmniPosition::readData()
@@ -39,4 +43,41 @@
     //assemble
     takeCounter = headerPoint;  //firstheader
     ++takeCounter;  //secondheader
+
+    ++takeCounter;
+    data[0] = buffer[(int)takeCounter];
+    ++takeCounter;
+    data[1] = buffer[(int)takeCounter];
+    X = ((data[0]<<8)|data[1]) - 32768;
+
+    ++takeCounter;
+    data[0] = buffer[(int)takeCounter];
+    ++takeCounter;
+    data[1] = buffer[(int)takeCounter];
+    Y = ((data[0]<<8)|data[1]) - 32768;
 }
+
+void OmniPosition::send()
+{
+    if(resetSend) {
+        putc('R');
+        resetSend = false;
+    } else {
+        //putc(0);
+    }
+}
+
+int OmniPosition::getX()
+{
+    return X;
+}
+
+int OmniPosition::getY()
+{
+    return Y;
+}
+
+void OmniPosition::reset()
+{
+    resetSend = true;
+}