Diff: RS405CB.cpp

Revision:

2:8ff319643fc3

Parent:

1:d9b19e1c1a9f

Child:

3:9311ea940978

diff -r d9b19e1c1a9f -r 8ff319643fc3 RS405CB.cpp
--- a/RS405CB.cpp	Sun Dec 08 09:38:19 2019 +0000
+++ b/RS405CB.cpp	Wed Dec 11 07:26:19 2019 +0000
@@ -106,6 +106,43 @@
     _permit = 0;
 }
 
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID ... 
+    speed ... 移動時間を10ms単位で指定
+
+Description:
+    
+----------------------------------------------------------------------------- */
+
+void RS405CB::SetSpeed(unsigned char ID, int speed)
+{
+    unsigned char TxData[10];
+    unsigned char CheckSum = 0;
+
+    TxData[0] = 0xFA;
+    TxData[1] = 0xAF;
+    TxData[2] = ID;
+    TxData[3] = 0x00;
+    TxData[4] = 0x1E + 2;
+    TxData[5] = 0x02;
+    TxData[6] = 0x01;
+    TxData[7] = (unsigned char)0x00FF & speed;
+    TxData[8] = (unsigned char)0x00FF & (speed >> 8);
+
+    for(int i=2; i<=8; i++)
+    {
+        CheckSum = CheckSum ^ TxData[i];
+    }
+    TxData[9] = CheckSum;
+
+    _permit = 1;
+    for(int i=0; i<=9; i++) {
+        putc(TxData[i]);
+    }
+    wait_us(810);
+    _permit = 0;
+}
 
 /* -----------------------------------------------------------------------------
 Arguments: