Diff: main.cpp

Revision:

15:0db5eea67306

Parent:

14:f3d19afbb99e

Child:

16:8c2eb2b4bead

--- a/main.cpp	Fri Aug 30 06:33:48 2019 +0000
+++ b/main.cpp	Mon Sep 02 05:32:28 2019 +0000
@@ -35,7 +35,7 @@
 int mode[4] = {0};
 
 std::vector<unsigned char> buf;
-uint8_t canbuf[3];
+uint8_t canbuf[4];
 CANMessage msg;
 bool testing = false;
 void start();
@@ -63,6 +63,7 @@
         canbuf[0] = msg.data[0];
         canbuf[1] = msg.data[1];
         canbuf[2] = msg.data[2];
+        canbuf[3] = msg.data[3];
         canRecieved = true;
     }
 }
@@ -84,7 +85,7 @@
 
 void processData(unsigned char data[])
 {
-    if(data[0]^data[1] == data[2]) {
+    if(data[0]^data[1]^data[2] == data[3]) {
         if((data[0]>>5) == addr) {
             addrChecked =true;
             pointedMotor = data[0]%4;
@@ -92,11 +93,11 @@
             motor[pointedMotor].braking = (data[0]>>3)%2;
 
             motor[pointedMotor].setMode(mode[pointedMotor]);
-            if(data[1] == 126) {
+            if((data[1] + (data[2]<<8)) == 32768) {
                 motor[pointedMotor].setMotorSpeed(0);
                 //serial.printf("STOP");
             } else {
-                motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
+                motor[pointedMotor].setMotorSpeed(((data[1] + (data[2]<<8))-32768.0)/32768.0);
             }
             addrChecked = false;
             headerRecieved = false;
@@ -114,24 +115,45 @@
         motor[i].setMode(SM);
         motor[i].setMotorSpeed(0);
     }
-    beep.period(1.0 / 2959.955);
-    beep = 0.2;
+    beep.period(1.0 / 3951.066);
+    beep = 0.5;
     wait(0.4);
     
     beep = 0.0;
     wait(0.1);
     
-    beep.period(1.0 / 2637.020);
-    beep = 0.2;
-    wait(0.4);
+    beep.period(1.0 / 3951.066);
+    beep = 0.5;
+    wait(0.2);
+    
+    beep = 0.0;
+    wait(0.1);
+    
+    beep.period(1.0 / 3951.066);
+    beep = 0.5;
+    wait(0.2);
     
     beep = 0.0;
     wait(0.1);
     
-    beep.period(1.0 / 3322.438);
+    beep.period(1.0 / 3951.066);
+    beep = 0.5;
+    wait(0.2);
+    
+    beep = 0.0;
+    wait(0.1);
+   
+    beep.period(1.0 / 3951.066);
+    beep = 0.5;
+    wait(0.4);
+   
+    beep = 0.0;
+    wait(0.1);
+ 
+    beep.period(1.0 / 2637.020);
     for(int i = 0; i<addr; i++) {
-        beep = 0.2;
-        wait(0.4);
+        beep = 0.1;
+        wait(0.2);
         beep = 0.0;
         wait(0.075);
     } // beep addr times    
@@ -142,7 +164,7 @@
 {
     start();
 
-    unsigned char tmp[3] = {0};
+    unsigned char tmp[4] = {0};
     addrChecked=false,headerRecieved=false;
     can.attach(&canRxIt, CAN::RxIrq);
     //motor[0].setMotorSpeed(0);
@@ -157,13 +179,14 @@
     beep = 0.0;
     while(1) {
         debugLED = addr;
-        if(buf.size() > 4) {
+        if(buf.size() > 5) {
             if (buf[0] == 255) {
                 tmp[0] = buf[1];
                 tmp[1] = buf[2];
                 tmp[2] = buf[3];
+                tmp[3] = buf[4];
                 processData(tmp);
-                buf.erase(buf.begin(),buf.begin()+3);
+                buf.erase(buf.begin(),buf.begin()+4);
                 debugLED0 = !debugLED0;
             } else {
                 buf.erase(buf.begin());
@@ -175,6 +198,7 @@
             tmp[0] = canbuf[0];
             tmp[1] = canbuf[1];
             tmp[2] = canbuf[2];
+            tmp[3] = canbuf[3];
             processData(tmp);
             debugLED0 = !debugLED0;
             canRecieved = false;