Diff: RS405CB.cpp

Revision:

1:d9b19e1c1a9f

Parent:

0:22a84e434ec8

Child:

2:7e4294a350fd

--- a/RS405CB.cpp	Sun Dec 08 06:20:03 2019 +0000
+++ b/RS405CB.cpp	Sun Dec 08 09:38:19 2019 +0000
@@ -68,6 +68,47 @@
 
 /* -----------------------------------------------------------------------------
 Arguments:
+    ID   ... 動かしたいサーボのID
+    data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
+    speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500)
+
+Description:
+    
+----------------------------------------------------------------------------- */
+void RS405CB::GoalPosition(unsigned char ID, int data, int speed)
+{
+    unsigned char TxData[12];
+    unsigned char CheckSum = 0;
+
+    TxData[0] = 0xFA;
+    TxData[1] = 0xAF;
+    TxData[2] = ID;
+    TxData[3] = 0x00;
+    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[10] = (unsigned char)0x00FF & (speed >> 8);
+
+    for(int i=2; i<=10; i++)
+    {
+        CheckSum = CheckSum ^ TxData[i];
+    }
+    TxData[11] = CheckSum;
+
+    _permit = 1;
+    for(int i=0; i<=11; i++) {
+        putc(TxData[i]);
+    }
+    wait_us(810);
+    _permit = 0;
+}
+
+
+/* -----------------------------------------------------------------------------
+Arguments:
     oldID ... IDを変更したいサーボの現在のID
     newID ... 新しいID．最大は255