Revision 4:8289dcb78a23, committed 2019-12-12

Comitter:

FumiyaKoike

Date:

Thu Dec 12 04:44:52 2019 +0000

Parent:

3:f7eb7b062ae0

Child:

5:e6208d124ead

Commit message:

Add SetSpeed func

Changed in this revision

--- a/RS405CB.cpp	Wed Dec 11 06:23:57 2019 +0000
+++ b/RS405CB.cpp	Thu Dec 12 04:44:52 2019 +0000
@@ -166,38 +166,35 @@
 
 /* -----------------------------------------------------------------------------
 Arguments:
-    ID   ... 動かしたいサーボのID
-    data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
+    ID   ... 
     speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500)
 
 Description:
-    
+    サーボの移動時間を設定する
 ----------------------------------------------------------------------------- */
-void RS405CB::GoalTime(unsigned char ID, int data, int speed)
+void RS405CB::SetSpeed(unsigned char ID, int speed)
 {
-    unsigned char TxData[12];
+    unsigned char TxData[10];
     unsigned char CheckSum = 0;
 
     TxData[0] = 0xFA;
     TxData[1] = 0xAF;
     TxData[2] = ID;
     TxData[3] = 0x00;
-    TxData[4] = 0x1E;
-    TxData[5] = 0x04;
+    TxData[4] = 0x1E + 2;
+    TxData[5] = 0x02;
     TxData[6] = 0x01;
-    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);
+    TxData[7] = (unsigned char)0x00FF & speed;
+    TxData[8] = (unsigned char)0x00FF & (speed >> 8);
 
-    for(int i=2; i<=10; i++)
+    for(int i=2; i<=8; i++)
     {
         CheckSum = CheckSum ^ TxData[i];
     }
-    TxData[11] = CheckSum;
+    TxData[9] = CheckSum;
 
     _permit = 1;
-    for(int i=0; i<=11; i++) {
+    for(int i=0; i<=9; i++) {
         putc(TxData[i]);
     }
     wait_us(810);
@@ -207,55 +204,47 @@
 
 /* -----------------------------------------------------------------------------
 Arguments:
-    ID   ... 動かしたいサーボのIDの配列
-    data ... 目標角の配列．degreeの10倍の値を指定する．（ex 90° -> data = 900）
+    ID   ... 
     speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500)
 
 Description:
-    GoalPositionのロングパケットバージョン
+    
     
 ----------------------------------------------------------------------------- */
-void RS405CB::GoalTime(unsigned char* ID, int* data, int* speed)
+void RS405CB::SetSpeed(unsigned char* ID, int* speed);
 {
     const unsigned char size = sizeof(ID);
-    unsigned char TxData[8 + 5 * size];
+    unsigned char TxData[8 + 3 * 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
     TxData[2] = 0x00; // ID -> 0x00
     TxData[3] = 0x00; // Flags -> 0x00
-    TxData[4] = 0x1E; // Addr -> GoalPosition(0x1E)
-    TxData[5] = 0x05; // Length -> ID(1Byte) + GoalPosition_L,H(2Byte) + GoalSpeed_L,H(2Byte)
+    TxData[4] = 0x1E+2; // Addr -> GoalTime
+    TxData[5] = 0x03; // Length -> ID(1Byte) + GoalTime_L,H(2Byte)
     TxData[6] =  (unsigned char)size;  // Count -> Number of Servos
     
     for(int i=0; i<size; i++)
     {
-        TxData[7  + i*5] = ID[i];
-        TxData[8  + i*5] = (unsigned char)0x00FF & data[i];
-        TxData[9  + i*5] = (unsigned char)0x00FF & (data[i] >> 8);
-        TxData[10 + i*5] = (unsigned char)0x00FF & speed[i];
-        TxData[11 + i*5] = (unsigned char)0x00FF & (speed[i] >> 8);
+        TxData[7  + i*3] = ID[i];
+        TxData[8  + i*3] = (unsigned char)0x00FF & speed[i];
+        TxData[9  + i*3] = (unsigned char)0x00FF & (speed[i] >> 8);
     }
 
-    for(int i=2; i<=6+5*size; i++)
-    {
+    for(int i=2; i<=6+3*size; i++)
         CheckSum = CheckSum ^ TxData[i];
-    }
-    TxData[7 + 5*size] = CheckSum;
+        
+    TxData[7 + 3*size] = CheckSum;
 
     _permit = 1;
-    for(int i=0; i<=7+5*size; i++) {
+    
+    for(int i=0; i<=7+3*size; i++)
         putc(TxData[i]);
-    }
+        
     wait_us(810);
     _permit = 0;
     
-    wait_ms(max_speed * 10);
-    
 }
 
 

--- a/RS405CB.h	Wed Dec 11 06:23:57 2019 +0000
+++ b/RS405CB.h	Thu Dec 12 04:44:52 2019 +0000
@@ -11,8 +11,8 @@
         void TorqueOn(unsigned char* ID);
         void GoalPosition(unsigned char ID, int data);
         void GoalPosition(unsigned char* ID, int* data);
-        void GoalTime(unsigned char ID, int data, int speed);
-        void GoalTime(unsigned char* ID, int* data, int* speed);
+        void SetSpeed(unsigned char ID, int speed);
+        void SetSpeed(unsigned char* ID, int* speed);
         void Change_ID(unsigned char oldID, unsigned char newID);
         int  Read_Deg(unsigned char ID);
         int  Read_mA(unsigned char ID);