Diff: OmniPosition.cpp

Revision:

3:47676abdf529

Parent:

2:edd33d3ad0fd

Child:

4:fc4c88fffef8

--- a/OmniPosition.cpp	Sun Jul 01 16:39:13 2018 +0900
+++ b/OmniPosition.cpp	Tue Jul 31 08:30:35 2018 +0000
@@ -4,11 +4,10 @@
     RawSerial(serialTX, serialRX, DEFAULT_BAUD),
     readCounter(SERIAL_BUFFER_SIZE, 0),
     takeCounter(SERIAL_BUFFER_SIZE, 0),
-    X(0),
-    Y(0)
+    angle(0)
 {
-    buffer = new char[SERIAL_BUFFER_SIZE];
-    data = new char[2];
+    buffer = new uint8_t[SERIAL_BUFFER_SIZE];
+    data = new uint8_t[2];
     attach(callback(this, &OmniPosition::readData));
     assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
     sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ);
@@ -45,16 +44,16 @@
     ++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;
+    angleInt = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
+    angle = angleInt/100.0;
+    if(angle > 180){
+        angle = angle - 655; 
+        }
+    
 }
 
 void OmniPosition::send()
@@ -67,15 +66,11 @@
     }
 }
 
-int OmniPosition::getX()
+double OmniPosition::getAngle()
 {
-    return X;
+    return angle;
 }
 
-int OmniPosition::getY()
-{
-    return Y;
-}
 
 void OmniPosition::reset()
 {