Diff: ikarashiMDC.cpp

Revision:

14:d5ace3e9a620

Parent:

13:ea34af94e90c

Child:

15:3ae22f7861b4

--- a/ikarashiMDC.cpp	Sat Nov 11 10:50:30 2017 +0000
+++ b/ikarashiMDC.cpp	Fri Aug 30 06:42:26 2019 +0000
@@ -1,10 +1,8 @@
 #include "ikarashiMDC.h"
 
 
-ikarashiMDC::ikarashiMDC(DigitalOut* serialcontrol,uint8_t taddr,uint8_t tmotorNum,bool tmode,Serial *tserial)
+ikarashiMDC::ikarashiMDC(uint8_t taddr,uint8_t tmotorNum,bool tmode,Serial *tserial)
 {
-    serialControl = serialcontrol;
-    serialControl->write(true);
     serial = tserial;
     addr = taddr;
     motorNum = tmotorNum;
@@ -14,19 +12,20 @@
 
 int ikarashiMDC::setSpeed(const double& speed)
 {
-    uint8_t data[4],dataSpeed;
+    uint8_t data[5],dataSpeed;
     //Limiter
     const double cropped_speed = std::min(1.0,std::max(-1.0,speed));
     //dataspeed 0~253 neutaral 126
-    dataSpeed = ((cropped_speed+1.0)/2.0)*253;
+    dataSpeed = ((cropped_speed+1.0)/2.0)*0xffff;
     //printf("%d\n",dataSpeed);
     //set sending data
     data[0] = 255; //header
     data[1] = (addr<<5) + motorNum + (mode<<4)+(braking<<3); //address
-    data[2] = dataSpeed;
-    data[3] = data[1]^data[2];
+    data[2] = dataSpeed & 0xff;
+    data[3] = dataSpeed >> 8;
+    data[4] = data[1]^data[2]^data[3];
     //send data
-    for(int i=0; i<4; i++) {
+    for(int i=0; i<5; i++) {
         serial->putc(data[i]);
     }
     return 0;