/******************************************************************/
/*  動作確認済環境                                                */
/*    開発環境  ：Arduino 1.8.5                                   */
/*    ボード    ：Arduino UNO R3                                  */
/******************************************************************/

/* グローバル変数定義 */
int REDE = 2;                // デジタルPin2 を送信イネーブルピンに設定

/*-------------------------------------------------*/
/* 機能   : Arduino 初期化                         */
/* 名前   : setup                                  */
/* 引数   : 無し                                   */
/* 戻り値 : 無し                                   */
/*-------------------------------------------------*/
void setup(){
    pinMode(REDE, OUTPUT);     // デジタルPin2(REDE)を出力に設定
    Serial.begin(115200);     // ボーレート 115,200bps
}

/*-------------------------------------------------*/
/* 機能   : サーボトルク設定                       */
/* 名前   : 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);     // 送信許可
    for(int i=0; i<=8; i++){
        Serial.write(TxData[i]);
    }
    Serial.flush();              // データ送信完了待ち
    digitalWrite(REDE, LOW);      // 送信禁止
} 

/*-------------------------------------------------*/
/* 機能   : サーボ角度・速度指定                   */
/* 名前   : Move                                   */
/* 引数   : ID (Servo ID)                          */
/*        : Angle (Present Posion L&H)             */
/*        : Speed (Present Time L&H)               */
/* 戻り値 : 無し                                   */
/*-------------------------------------------------*/
void Move (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);     // 送信許可
    for(int i=0; i<=11; i++){
        Serial.write(TxData[i]);
    }
    Serial.flush();              // データ送信完了待ち
    digitalWrite(REDE, LOW);      // 送信禁止
}

/*-------------------------------------------------*/
/* 機能   : メインプログラム                       */ 
/* 名前   : loop                                   */
/* 引数   : 無し                                   */
/* 戻り値 : 無し                                   */
/*-------------------------------------------------*/
void loop(){
    setup();                    // Arduino 初期化
    delay(100);                 // wait (100msec)
    Torque(0x01, 0x01);         // ID = 1(0x01) , torque = ON   (0x01)
                                // torque = OFF(0x00), ON(0x01), BRAKE(0x02)
    delay(100);                 // wait (100msec)
    
    while(1){
        Move(1,300,100);        // ID = 1 , GoalPosition = 30.0deg(300) , Time = 1.0sec(100)
        delay(2000);            // wait (2sec)
        Move(1,-300,100);        // ID = 1 , GoalPosition = -30.0deg(300) , Time = 1.0sec(100)
        delay(2000);            // wait (2sec)
    }
}