Diff: ikarashiMDC.cpp

Revision:

1:5e8014a1adbd

Parent:

0:314f2bed3958

Child:

2:75153205d6e8

--- a/ikarashiMDC.cpp	Mon Aug 07 22:39:44 2017 +0000
+++ b/ikarashiMDC.cpp	Thu Aug 10 07:17:14 2017 +0000
@@ -3,9 +3,11 @@
 
 ikarashiMDC::ikarashiMDC()
 {
+    serialControl = new DigitalOut(D2);
     serial=NULL;
     addr = 0;
     motorNum = 0;
+    serialControl->write(true);
 }
 
 int ikarashiMDC::setMotorinfo(uint8_t _addr,uint8_t _motorNum,Serial *_serial)
@@ -27,7 +29,7 @@
     dataSpeed = ((speed+1.0)/2.0)*253;
     //set sending data
     data[0] = 255; //header
-    data[1] = addr; //address
+    data[1] = addr<<4 + motorNum; //address
     data[2] = dataSpeed;
     
     //send data
@@ -36,4 +38,36 @@
         serial->putc(data[i]);
     }
     return 0;
- }
\ No newline at end of file
+ }
+void ikarashiMDC::stop() 
+{
+    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]);
+            }
+        }
+    }
+     
+}
+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