Diff: OmniPosition.cpp

Revision:

5:f8c3aeb4e65f

Parent:

4:fc4c88fffef8

Child:

6:f8dbbe93bc7b

--- a/OmniPosition.cpp	Tue Aug 21 13:41:25 2018 +0900
+++ b/OmniPosition.cpp	Wed Aug 22 20:21:48 2018 +0900
@@ -1,24 +1,24 @@
 #include "OmniPosition.h"
 
 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
-    RawSerial(serialTX, serialRX, DEFAULT_BAUD),
-    readCounter(SERIAL_BUFFER_SIZE, 0),
-    takeCounter(SERIAL_BUFFER_SIZE, 0),
+    RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD),
+    readCounter(0),
+    takeCounter(0),
     X(0),
     Y(0),
     theta(0.0)
 {
-    buffer = new char[SERIAL_BUFFER_SIZE];
+    buffer = new char[OP_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);
+    assembleTicker.attach(callback(this, &OmniPosition::assemble), OP_RECEIVE_FREQ);
+    sendTicker.attach(callback(this, &OmniPosition::send), OP_SEND_FREQ);
 }
 
 void OmniPosition::readData()
 {
     buffer[(int)readCounter] = getc();
-    ++readCounter;
+    readCounter = incrementCounter(readCounter);
 }
 
 void OmniPosition::assemble()
@@ -27,11 +27,11 @@
     headerCheck = false;
     headerPoint = 0xff;
 
-    for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) {
-        if(buffer[i] == HEADER_FIRST_BYTE) {
+    for(int i = 0; i < OP_SERIAL_BUFFER_SIZE; i++) {
+        if(buffer[i] == OP_HEADER_FIRST_BYTE) {
             takeCounter = i;
-            ++takeCounter;
-            if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) {
+            takeCounter = incrementCounter(takeCounter);
+            if(buffer[(int)takeCounter] == OP_HEADER_SECOND_BYTE) {
                 headerCheck = true;
                 headerPoint = i;
             }
@@ -44,26 +44,26 @@
     //assemble
     checksum = 0;
     takeCounter = headerPoint;  //firstheader
-    ++takeCounter;  //secondheader
+    takeCounter = incrementCounter(takeCounter);  //secondheader
 
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     data[0] = buffer[(int)takeCounter];
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     data[1] = buffer[(int)takeCounter];
     X = ((data[0]<<8)|data[1]) - 32768;
     checksum = (data[0] ^ data[1]);
 
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     data[0] = buffer[(int)takeCounter];
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     data[1] = buffer[(int)takeCounter];
     Y = ((data[0]<<8)|data[1]) - 32768;
     checksum = (checksum ^ data[0]);
     checksum = (checksum ^ data[1]);
 
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     data[0] = buffer[(int)takeCounter];
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     data[1] = buffer[(int)takeCounter];
     thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
     theta = thetaint / 100.0;
@@ -78,7 +78,7 @@
     checksum = (checksum ^ data[0]);
     checksum = (checksum ^ data[1]);
 
-    ++takeCounter;
+    takeCounter = incrementCounter(takeCounter);  //secondheader
     if(buffer[(int)takeCounter] != checksum) {
         X = bfrX;
         Y = bfrY;
@@ -91,6 +91,15 @@
     
 }
 
+int OmniPosition::incrementCounter(int counter)
+{
+    ++counter;
+    if(counter >= OP_SERIAL_BUFFER_SIZE) {
+        counter -= OP_SERIAL_BUFFER_SIZE;
+    }
+    return counter;
+}
+
 void OmniPosition::send()
 {
     if(resetSend) {