Diff: RS405CB.cpp

Revision:

3:f7eb7b062ae0

Parent:

2:7e4294a350fd

Child:

4:8289dcb78a23

--- a/RS405CB.cpp	Mon Dec 09 08:28:56 2019 +0000
+++ b/RS405CB.cpp	Wed Dec 11 06:23:57 2019 +0000
@@ -106,9 +106,8 @@
     TxData[8] = (unsigned char)0x00FF & (data >> 8);
 
     for(int i=2; i<=8; i++)
-    {
         CheckSum = CheckSum ^ TxData[i];
-    }
+        
     TxData[9] = CheckSum;
 
     _permit = 1;
@@ -156,9 +155,10 @@
     TxData[7 + 3*size] = CheckSum;
 
     _permit = 1;
-    for(int i=0; i<=7+3*size; i++) {
+    
+    for(int i=0; i<=7+3*size; i++)
         putc(TxData[i]);
-    }
+        
     wait_us(810);
     _permit = 0;
 }
@@ -173,7 +173,7 @@
 Description:
     
 ----------------------------------------------------------------------------- */
-void RS405CB::GoalPosition(unsigned char ID, int data, int speed)
+void RS405CB::GoalTime(unsigned char ID, int data, int speed)
 {
     unsigned char TxData[12];
     unsigned char CheckSum = 0;
@@ -185,9 +185,9 @@
     TxData[4] = 0x1E;
     TxData[5] = 0x04;
     TxData[6] = 0x01;
-    TxData[7] =  (unsigned char)0x00FF & data;
-    TxData[8] =  (unsigned char)0x00FF & (data >> 8);
-    TxData[9] =  (unsigned char)0x00FF & speed;
+    TxData[7] = (unsigned char)0x00FF & data;
+    TxData[8] = (unsigned char)0x00FF & (data >> 8);
+    TxData[9] = (unsigned char)0x00FF & speed;
     TxData[10] = (unsigned char)0x00FF & (speed >> 8);
 
     for(int i=2; i<=10; i++)
@@ -204,6 +204,7 @@
     _permit = 0;
 }
 
+
 /* -----------------------------------------------------------------------------
 Arguments:
     ID   ... 動かしたいサーボのIDの配列
@@ -214,11 +215,14 @@
     GoalPositionのロングパケットバージョン
     
 ----------------------------------------------------------------------------- */
-void RS405CB::GoalPosition(unsigned char* ID, int* data, int* speed)
+void RS405CB::GoalTime(unsigned char* ID, int* data, int* speed)
 {
     const unsigned char size = sizeof(ID);
     unsigned char TxData[8 + 5 * size];
     unsigned char CheckSum = 0;
+    int max_speed;
+    for(int i=0; i<sizeof(speed); i++)
+        max_speed = max(speed[i], max_speed);
 
     TxData[0] = 0xFA; //header
     TxData[1] = 0xAF; //header
@@ -249,6 +253,9 @@
     }
     wait_us(810);
     _permit = 0;
+    
+    wait_ms(max_speed * 10);
+    
 }