/******************************************************************/
/* Futabaのサンプルプログラム(for Arduino)を若干変更して使用。*/
/* https://www.futaba.co.jp/img/uploads/files/robot/Futaba_Sample_Arduino_RS30x_Program_rev2.pdf
/* ボード ：ESP32 Dev Module, UART2(Tx:GPIO17) */
/* 電源投入順序 */
/* 1. サーボ, (ID=1 工場出荷状態)*/
/* 2. ESP32 */
/******************************************************************/
//int REDE = 2; // デジタルPin2 をRS485出力イネーブルピンに設定
/*-------------------------------------------------*/
/* 機能 : Arduino 初期化 */
/* 名前 : setup */
/* 引数 : 無し */
/* 戻り値 : 無し */
/*-------------------------------------------------*/

void setup (){
//pinMode(REDE, OUTPUT); // デジタルPin2(REDE)を出力に設定
Serial.begin(115200);
Serial2.begin(115200); // ボーレート 115,200bps
delay(500); // 500msディレイ
}
/*-------------------------------------------------*/
/* 機能 : サーボトルク設定 */
/* 名前 : torque */
/* 引数 : ID (Servo ID) */
/* : data (Torque enable) */
/* 戻り値 : 無し */
/*-------------------------------------------------*/
void torque (unsigned char ID, unsigned char data){
unsigned char TxData[9]; //送信データバッファ [9byte]
unsigned char CheckSum = 0; // チェックサム計算用変数
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x24; // Address
TxData[5] = 0x01; // Length
TxData[6] = 0x01; // Count
TxData[7] = data; // Data
// チェックサム計算
for(int i=2; i<=7; i++){
CheckSum = CheckSum ^ TxData[i]; // ID～DATAまでのXOR
}
TxData[8] = CheckSum; //Sum
// パケットデータ送信
//digitalWrite(REDE, HIGH); // RS485 送信許可 (TTLの場合は必要無し)
for(int i=0; i<=8; i++){
Serial2.write(TxData[i]);
}
delayMicroseconds(250); // データ送信完了待ち
//digitalWrite(REDE, LOW); // RS485 送信禁止 (TTLの場合は必要無し)
}
/*-------------------------------------------------*/
/* 機能 : サーボ角度・速度指定 */
/* 名前 : Move_SV */
/* 引数 : ID (Servo ID) */
/* : Angle (Present Posion L&H) */
/* : Speed (Present Time L&H) */
/* 戻り値 : --- */
/*-------------------------------------------------*/
void Move_SV (unsigned char ID, int Angle, int Speed){
unsigned char TxData[12]; //送信データバッファ [12byte]
unsigned char CheckSum = 0; // チェックサム計算用変数
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x1E; // Address
TxData[5] = 0x04; // Length
TxData[6] = 0x01; // Count
// Angle
TxData[7] = (unsigned char)0x00FF & Angle; // Low byte
TxData[8] = (unsigned char)0x00FF & (Angle >> 8); //Hi byte
// Speed
TxData[9] = (unsigned char)0x00FF & Speed; // Low byte
TxData[10] = (unsigned char)0x00FF & (Speed >> 8); //Hi byte
// チェックサム計算
for(int i=2; i<=10; i++){
CheckSum = CheckSum ^ TxData[i]; // ID～DATAまでのXOR
}
TxData[11] = CheckSum; //Sum
// パケットデータ送信
//digitalWrite(REDE, HIGH); // RS485 送信許可 (TTLの場合は必要無し)
for(int i=0; i<=11; i++){
Serial2.write(TxData[i]);
}
delayMicroseconds(250); // データ送信完了待ち
//digitalWrite(REDE, LOW); // RS485 送信禁止 (TTLの場合は必要無し)
}
/*-------------------------------------------------*/
/* 機能 : メインプログラム */
/* 名前 : loop */
/* 引数 : 無し */
/* 戻り値 : 無し */
/*-------------------------------------------------*/
void loop (){
Serial.println("Demo test");
unsigned char i = 0; // for文用
//setup (); // Arduino 初期化
torque(0x01, 0x01); // ID = 1(0x01) torque = OFF(0x00), ON(0x01), BRAKE(0x02)
delay(1000); // wait (1sec)
//while(1){
//for(i=0; i<=20; i++){
Move_SV(0x01,300,100); //ID = 1～20 , GoalPosition = 30.0deg(300) , Time = 1.0sec(100)
delay(500); // wait (0.5sec)
//}
delay(500); // wait (0.5sec)
Move_SV(0x01,-300,100); //ID = 1(0x01) , GoalPosition = -30.0deg(-300) , Time = 1.0sec(100)
delay(2000); // wait (2.0sec)
//}
}