Diff: RS405CB.cpp

Revision:

2:7e4294a350fd

Parent:

1:d9b19e1c1a9f

Child:

3:f7eb7b062ae0

diff -r d9b19e1c1a9f -r 7e4294a350fd RS405CB.cpp
--- a/RS405CB.cpp	Sun Dec 08 09:38:19 2019 +0000
+++ b/RS405CB.cpp	Mon Dec 09 08:28:56 2019 +0000
@@ -31,6 +31,59 @@
 
 /* -----------------------------------------------------------------------------
 Arguments:
+    ID   ... トルクをオンに設定したいサーボのID
+    
+Description:
+    トルクをオンにする
+----------------------------------------------------------------------------- */
+void RS405CB::TorqueOn(unsigned char ID)
+{
+    unsigned char TxData[9]= {0xFA, 0xAF, ID, 0x00, 0x24, 0x01, 0x01, 0x01, 0};
+    unsigned char CheckSum = 0;
+    for(int i = 2; i <= 7; i++) CheckSum = CheckSum ^ TxData[i];
+    TxData[8] = CheckSum;
+    _permit = 1;
+    for (int i=0; i<=8; i++) putc(TxData[i]);
+    wait_us(750);
+    _permit = 0;
+}
+
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID   ... トルクをオンに設定したいサーボのIDの配列
+    
+Description:
+    トルクをオンにする
+----------------------------------------------------------------------------- */
+void RS405CB::TorqueOn(unsigned char* ID)
+{
+    const unsigned char size = sizeof(ID);
+    unsigned char TxData[8 + 2*size];
+    TxData[0] = 0xFA; //header
+    TxData[1] = 0xAF; //header
+    TxData[2] = 0x00; // ID -> 0x00
+    TxData[3] = 0x00; // Flags -> 0x00
+    TxData[4] = 0x24; // Addr -> Torque(0x24)
+    TxData[5] = 0x02; // Length -> ID(1Byte) + Torque(1Byte)
+    TxData[6] =  (unsigned char)size;  // Count -> Number of Servos
+    
+    for(int i=0; i<size; i++)
+    {
+        TxData[7 + 2*i] = ID[i];
+        TxData[8 + 2*i] = 0x01;
+    }
+    unsigned char CheckSum = 0;
+    for(int i=2; i<=6+2*size; i++)
+        CheckSum = CheckSum ^ TxData[i];
+    TxData[7+2*size] = CheckSum;
+    _permit = 1;
+    for (int i=0; i<=7+2*size; i++) putc(TxData[i]);
+    wait_us(750);
+    _permit = 0;
+}
+
+/* -----------------------------------------------------------------------------
+Arguments:
     ID   ... 動かしたいサーボのID
     data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
 
@@ -68,6 +121,51 @@
 
 /* -----------------------------------------------------------------------------
 Arguments:
+    ID   ... 動かしたいサーボのIDの配列
+    data ... 目標角の配列．degreeの10倍の値を指定する．（ex 90° -> data = 900）
+    speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500)
+
+Description:
+    GoalPositionのロングパケットバージョン
+    
+----------------------------------------------------------------------------- */
+void RS405CB::GoalPosition(unsigned char* ID, int* data)
+{
+    const unsigned char size = sizeof(ID);
+    unsigned char TxData[8 + 3 * size];
+    unsigned char CheckSum = 0;
+
+    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] = 0x03; // Length -> ID(1Byte) + GoalPosition_L,H(2Byte)
+    TxData[6] =  (unsigned char)size;  // Count -> Number of Servos
+    
+    for(int i=0; i<size; i++)
+    {
+        TxData[7  + i*3] = ID[i];
+        TxData[8  + i*3] = (unsigned char)0x00FF & data[i];
+        TxData[9  + i*3] = (unsigned char)0x00FF & (data[i] >> 8);
+    }
+
+    for(int i=2; i<=6+3*size; i++)
+        CheckSum = CheckSum ^ TxData[i];
+        
+    TxData[7 + 3*size] = CheckSum;
+
+    _permit = 1;
+    for(int i=0; i<=7+3*size; i++) {
+        putc(TxData[i]);
+    }
+    wait_us(810);
+    _permit = 0;
+}
+
+
+/* -----------------------------------------------------------------------------
+Arguments:
     ID   ... 動かしたいサーボのID
     data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
     speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500)
@@ -87,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++)
@@ -106,6 +204,53 @@
     _permit = 0;
 }
 
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID   ... 動かしたいサーボのIDの配列
+    data ... 目標角の配列．degreeの10倍の値を指定する．（ex 90° -> data = 900）
+    speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500)
+
+Description:
+    GoalPositionのロングパケットバージョン
+    
+----------------------------------------------------------------------------- */
+void RS405CB::GoalPosition(unsigned char* ID, int* data, int* speed)
+{
+    const unsigned char size = sizeof(ID);
+    unsigned char TxData[8 + 5 * size];
+    unsigned char CheckSum = 0;
+
+    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[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);
+    }
+
+    for(int i=2; i<=6+5*size; i++)
+    {
+        CheckSum = CheckSum ^ TxData[i];
+    }
+    TxData[7 + 5*size] = CheckSum;
+
+    _permit = 1;
+    for(int i=0; i<=7+5*size; i++) {
+        putc(TxData[i]);
+    }
+    wait_us(810);
+    _permit = 0;
+}
+
 
 /* -----------------------------------------------------------------------------
 Arguments: