Diff: RS405CB.cpp

Revision:

0:22a84e434ec8

Child:

1:d9b19e1c1a9f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RS405CB.cpp	Sun Dec 08 06:20:03 2019 +0000
@@ -0,0 +1,254 @@
+#include "mbed.h"
+#include "RS405CB.h"
+
+RS405CB::RS405CB(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit)
+{
+    baud(115200);
+    format(8,Serial::None,1);
+}
+
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID   ... トルクを設定したいサーボのID
+    data ... トルクの状態の設定
+             0x00 OFF
+             0x01 ON
+             0x02 BREKE
+Description:
+    トルク設定。入力されたIDのサーボモータのトルクを制御する
+----------------------------------------------------------------------------- */
+void RS405CB::Torque(unsigned char ID, unsigned char data)
+{
+    unsigned char TxData[9]= {0xFA, 0xAF, ID, 0x00, 0x24, 0x01, 0x01, data, 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((char)TxData[i]);
+    wait_us(750);
+    _permit = 0;
+}
+
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID   ... 動かしたいサーボのID
+    data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
+
+Description:
+    
+----------------------------------------------------------------------------- */
+void RS405CB::GoalPosition(unsigned char ID, int data)
+{
+    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] = 0x02;
+    TxData[6] = 0x01;
+    TxData[7] = (unsigned char)0x00FF & data;
+    TxData[8] = (unsigned char)0x00FF & (data >> 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:
+    oldID ... IDを変更したいサーボの現在のID
+    newID ... 新しいID．最大は255
+
+Description:
+    
+----------------------------------------------------------------------------- */
+void RS405CB::Change_ID(unsigned char oldID, unsigned char newID){
+    unsigned char TxData[9];
+    unsigned char CheckSum = 0;
+
+    TxData[0] = 0xFA;
+    TxData[1] = 0xAF;
+    TxData[2] = oldID;
+    TxData[3] = 0x00;
+    TxData[4] = 0x04;
+    TxData[5] = 0x01;
+    TxData[6] = 0x01;
+    TxData[7] = newID;
+    
+    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(810);
+    _permit = 0;
+
+    _ROM_Update(newID);
+}
+
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID ... 角度を読みたいサーボのID 
+
+Return:
+    get_deg(int) ... サーボの角度
+
+Description:
+    
+----------------------------------------------------------------------------- */
+int RS405CB::Read_Deg(unsigned char ID)
+{
+    unsigned char CheckSum=0;
+    int get_deg = 0;
+    unsigned char RxData[8];
+    unsigned char TxData[8];
+    
+    TxData[0] = 0xFA;
+    TxData[1] = 0xAF;
+    TxData[2] = ID;
+    TxData[3] = 0x0F;
+    TxData[4] = 0x2A;
+    TxData[5] = 0x02;
+    TxData[6] = 0x00;
+
+    for(int j=2; j<=6; j++) {
+        CheckSum = CheckSum ^ TxData[j];
+    }
+    TxData[7]=CheckSum;
+
+    _permit = 1;
+    for (int j=0; j<=7; j++)
+        putc(TxData[j]);
+    wait_us(670);
+    _permit = 0;
+
+    CheckSum = 0;
+    while(getc() != 0xFD);
+    while(getc() != 0xDF);
+
+    for(int j=0; j<=6; j++){
+        RxData[j] = getc();
+        CheckSum = CheckSum ^ RxData[j];
+    }
+    
+    if(CheckSum == getc()){
+        get_deg = (int)RxData[6];
+        get_deg = get_deg << 8;
+        get_deg |= (int)RxData[5];
+    }
+        
+    return _s16(get_deg);
+}
+
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID ... 電流を読みたいサーボのID 
+
+Return:
+    get_mA(int) ... サーボに流れている電流量
+
+Description:
+    メーカー曰く，参考程度の値なので注意
+
+----------------------------------------------------------------------------- */
+int RS405CB::Read_mA(unsigned char ID)
+{
+    unsigned char CheckSum=0;
+    int get_mA = 0;
+    unsigned char RxData[8];
+    unsigned char TxData[8];
+    TxData[0] = 0xFA;
+    TxData[1] = 0xAF;
+    TxData[2] = ID;
+    TxData[3] = 0x0F;
+    TxData[4] = 0x30;
+    TxData[5] = 0x02;
+    TxData[6] = 0x00;
+
+    for(int j=2; j<=6; j++)
+    {
+        CheckSum = CheckSum ^ TxData[j];
+    }
+    TxData[7]=CheckSum;
+
+    _permit = 1;
+    for (int j=0; j<=7; j++)
+        putc(TxData[j]);
+    wait_us(670);
+    _permit = 0;
+
+    CheckSum = 0;
+    while(getc() != 0xFD);
+    while(getc() != 0xDF);
+
+    for(int j=0; j<=6; j++){
+        RxData[j] = getc();
+        CheckSum = CheckSum ^ RxData[j];
+    }
+    
+    if(CheckSum == getc()){
+        get_mA = (int)RxData[6];
+        get_mA = get_mA << 8;
+        get_mA |= (int)RxData[5];
+    }
+        
+    return _s16(get_mA);
+}
+
+
+/* -----------------------------------------------------------------------------
+Arguments:
+    ID ... ROMをアップデートしたいサーボのID
+
+Description:
+    ROM領域の値を変更した時に実行する．
+
+----------------------------------------------------------------------------- */
+void RS405CB::_ROM_Update(unsigned char ID){
+    unsigned char TxData[8];
+    unsigned char CheckSum = 0;
+
+    TxData[0] = 0xFA;
+    TxData[1] = 0xAF;
+    TxData[2] = ID;
+    TxData[3] = 0x40;
+    TxData[4] = 0xFF;
+    TxData[5] = 0x00;
+    TxData[6] = 0x00;
+    
+    for(int i=2; i<=6; i++)
+    {
+        CheckSum = CheckSum ^ TxData[i];
+    }
+    TxData[7] = CheckSum;
+    
+    _permit = 1;
+    for(int i=0; i<=7; i++) {
+        putc(TxData[i]);
+    }
+    wait_us(810);
+    _permit = 0;
+    wait(1.5);
+}
+
+int RS405CB::_s16(int value)
+{
+    return -(value & 0b1000000000000000)|(value & 0b0111111111111111);
+}