Diff: ikarashiMDC.cpp

Revision:

2:75153205d6e8

Parent:

1:5e8014a1adbd

Child:

4:d6f0b74affd7

Child:

5:b43c03ee7f8b

Child:

7:a8fcb8df927a

--- a/ikarashiMDC.cpp	Thu Aug 10 07:17:14 2017 +0000
+++ b/ikarashiMDC.cpp	Tue Aug 22 01:44:17 2017 +0000
@@ -1,73 +1,44 @@
 #include "ikarashiMDC.h"
 
 
-ikarashiMDC::ikarashiMDC()
+ikarashiMDC::ikarashiMDC(DigitalOut* serialcontrol,uint8_t taddr,uint8_t tmotorNum,bool tmode,Serial *tserial)
 {
-    serialControl = new DigitalOut(D2);
-    serial=NULL;
-    addr = 0;
-    motorNum = 0;
+    serialControl = serialcontrol;
     serialControl->write(true);
+    serial = tserial;
+    addr = taddr;
+    motorNum = tmotorNum;
+    mode = tmode;
 }
 
-int ikarashiMDC::setMotorinfo(uint8_t _addr,uint8_t _motorNum,Serial *_serial)
+int ikarashiMDC::setSpeed(const double& speed)
 {
-    serial = _serial;
-    addr = _addr;
-    motorNum = _motorNum;
+    uint8_t data[4],dataSpeed;
+    //Limiter
+    const double cropped_speed = -1 < speed ?
+                                  1 > speed ?
+                                  speed:1
+                                  :-1;
+
+    //dataspeed 0~253 neutaral 126
+    dataSpeed = ((cropped_speed+1.0)/2.0)*253;
+    //printf("%d\n",dataSpeed);
+    //set sending data
+    data[0] = 255; //header
+    data[1] = (addr<<5) + motorNum + (mode<<4); //address
+    data[2] = dataSpeed;
+    //send data
+    for(int i=0; i<4; i++) {
+        serial->putc(data[i]);
+    }
     return 0;
 }
 
-int ikarashiMDC::setMotorSpeed(double speed)
-{
-    uint8_t data[3],dataSpeed;
-    //Limiter
-    if(speed > 1.0) speed = 1.0;
-    if(speed < -1.0) speed = -1.0;
-    
-    //dataspeed 0~253 neutaral 126 
-    dataSpeed = ((speed+1.0)/2.0)*253;
-    //set sending data
-    data[0] = 255; //header
-    data[1] = addr<<4 + motorNum; //address
-    data[2] = dataSpeed;
-    
-    //send data
-    for(int i=0;i<3; i++)
-    {
-        serial->putc(data[i]);
-    }
-    return 0;
- }
-void ikarashiMDC::stop() 
+
+void estop(Serial *serial)
 {
-    int data[3];
-    data[0] = 255;
-    data[2] = 126;
-    for(int i = 0; i < 16;i++)
-    {
-        for(int j = 0;j <3;j++)
-        {
-            for(int k = 0;j<3;j++)
-            {
-                data[1] = i<<4 + j;
-                serial->putc(data[k]);
-            }
-        }
+    for(int i = 0; i<3; i++) {
+        serial->putc(255);
     }
-     
-}
-int ikarashiMDC::checkcomm()
-{
-    int data[3];
-    data[0] =255;
-    data[1] = addr<<4 +motorNum;
-    data[2]=126;
-    for(int i=0;i<3;i++)
-        serial->putc(data[i]);
-    serialControl->write(false);
-    if (serial->readable())
-        return serial->getc();
-    serialControl->write(true);
-    return false;
+
 }
\ No newline at end of file