Diff: ikarashiMDC.cpp

Revision:

12:3fa4f849d6f7

Parent:

9:12ea2a3883bb

Child:

13:ea34af94e90c

--- a/ikarashiMDC.cpp	Sat Sep 02 14:34:55 2017 +0000
+++ b/ikarashiMDC.cpp	Sat Nov 11 09:42:04 2017 +0900
@@ -14,12 +14,9 @@
 
 int ikarashiMDC::setSpeed(const double& speed)
 {
-    uint8_t data[3],dataSpeed;
+    uint8_t data[4],dataSpeed;
     //Limiter
-    const double cropped_speed = -1 < speed ?
-                                  1 > speed ?
-                                  speed:1
-                                  :-1;
+    const double cropped_speed = fmin(1.0,fmax(-1.0,speed);
 
     //dataspeed 0~253 neutaral 126
     dataSpeed = ((cropped_speed+1.0)/2.0)*253;
@@ -28,8 +25,9 @@
     data[0] = 255; //header
     data[1] = (addr<<5) + motorNum + (mode<<4)+(braking<<3); //address
     data[2] = dataSpeed;
+    data[3] = data[1]^data[2];
     //send data
-    for(int i=0; i<3; i++) {
+    for(int i=0; i<4; i++) {
         serial->putc(data[i]);
     }
     return 0;