Diff: SpeedMsg.cpp

Revision:

3:4a4463380739

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpeedMsg.cpp	Sat Mar 11 10:03:52 2017 +0000
@@ -0,0 +1,44 @@
+#include "SpeedMsg.h"
+
+SpeedMsg::SpeedMsg () {
+
+    counter1 = counter2 = 0;
+    //c2inc = { 1, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2, 1, 2, 1, 2, 2 };
+    char x = 0;
+    for (int i = 0; i < 16; i++) {
+        c2inc[i] = 1 + x;
+        if (i != 5 && i != 14) x = 1 - x;
+    }        
+    c2incp = 0;
+    
+    spb[0] = 0; spb[1] = 0;
+    spb[2] = 0; spb[3] = 0;
+    spb[4] = 0; spb[5] = 0;
+}
+
+void SpeedMsg::setSpeed(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5) {
+
+    spb[0] = b0; spb[1] = b1;
+    spb[2] = b2; spb[3] = b3;
+    spb[4] = b4; spb[5] = b5;
+}
+
+bool SpeedMsg::sendMessage(CAN *can) {
+
+    uint8_t data[8];
+    
+    data[0] = spb[0]; data[1] = spb[1];
+    data[2] = spb[2]; data[3] = spb[3];
+    data[4] = spb[4]; data[5] = spb[5];
+    
+    data[6] = counter1;
+    data[7] = 0xF0 + counter2;
+    
+    bool ok = can->write(CANMessage(0x1A6, (char*) data, 8));
+
+    counter1 = counter1 + 0x90;
+    counter2 = (counter2 + c2inc[c2incp++]) & 0x0f;
+    if (c2incp > 15) c2incp = 0;
+
+    return ok;
+}