Diff: OmniPosition.cpp

Revision:

4:fc4c88fffef8

Parent:

3:47676abdf529

Child:

5:f8c3aeb4e65f

diff -r 47676abdf529 -r fc4c88fffef8 OmniPosition.cpp
--- a/OmniPosition.cpp	Tue Jul 31 08:30:35 2018 +0000
+++ b/OmniPosition.cpp	Tue Aug 21 13:41:25 2018 +0900
@@ -4,10 +4,12 @@
     RawSerial(serialTX, serialRX, DEFAULT_BAUD),
     readCounter(SERIAL_BUFFER_SIZE, 0),
     takeCounter(SERIAL_BUFFER_SIZE, 0),
-    angle(0)
+    X(0),
+    Y(0),
+    theta(0.0)
 {
-    buffer = new uint8_t[SERIAL_BUFFER_SIZE];
-    data = new uint8_t[2];
+    buffer = new char[SERIAL_BUFFER_SIZE];
+    data = new char[2];
     attach(callback(this, &OmniPosition::readData));
     assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
     sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ);
@@ -40,19 +42,52 @@
     }
 
     //assemble
+    checksum = 0;
     takeCounter = headerPoint;  //firstheader
     ++takeCounter;  //secondheader
 
     ++takeCounter;
+    data[0] = buffer[(int)takeCounter];
+    ++takeCounter;
+    data[1] = buffer[(int)takeCounter];
+    X = ((data[0]<<8)|data[1]) - 32768;
+    checksum = (data[0] ^ data[1]);
+
+    ++takeCounter;
+    data[0] = buffer[(int)takeCounter];
+    ++takeCounter;
+    data[1] = buffer[(int)takeCounter];
+    Y = ((data[0]<<8)|data[1]) - 32768;
+    checksum = (checksum ^ data[0]);
+    checksum = (checksum ^ data[1]);
+
     ++takeCounter;
     data[0] = buffer[(int)takeCounter];
     ++takeCounter;
     data[1] = buffer[(int)takeCounter];
-    angleInt = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
-    angle = angleInt/100.0;
-    if(angle > 180){
-        angle = angle - 655; 
-        }
+    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;
+    if(buffer[(int)takeCounter] != checksum) {
+        X = bfrX;
+        Y = bfrY;
+        theta = bfrTheta;
+    } else {
+        bfrX = X;
+        bfrY = Y;
+        bfrTheta = theta;
+    }
     
 }
 
@@ -66,13 +101,23 @@
     }
 }
 
-double OmniPosition::getAngle()
+int OmniPosition::getX()
 {
-    return angle;
+    return X;
 }
 
+int OmniPosition::getY()
+{
+    return Y;
+}
+
+double OmniPosition::getTheta()
+{
+    return -theta;
+}
 
 void OmniPosition::reset()
 {
     resetSend = true;
 }
+