/******************************************************************/
/*  動作確認済環境                                                */
/*    開発環境  ：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);      // 送信禁止
}

/*-------------------------------------------------*/
/* 機能   : 角度データ取得
/* 名前   : ReadAngle
/* 引数   : ID (Servo ID)
/* 戻り値 : 無し
/*-------------------------------------------------*/
void ReadAngle (unsigned char ID){
    unsigned char TxData[8];    // 送信データバッファ [8byte]
    unsigned char CheckSum = 0; // チェックサム計算用変数

    // パケットデータ生成
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x0F;           // Flags
    TxData[4] = 0x2A;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x00;           // Count
    // チェックサム計算
    for(int i=2; i<=6; i++){
        CheckSum = CheckSum ^ TxData[i]; // ID～DATAまでのXOR
    }
    TxData[7] = CheckSum;       // Sum
    
    // パケットデータ送信
    digitalWrite(REDE, HIGH);   // 送信許可
    for(int i=0; i<=7; i++){
        Serial.write(TxData[i]);
    }
    Serial.flush();              // データ送信完了待ち
    digitalWrite(REDE, LOW);    // 送信禁止
}

/*----------------------------------------------------*/
/* 機能   : 角度データ受信
/* 名前   : WaitReadAngle
/* 引数   : 無し
/* 戻り値 : 角度データ
/*----------------------------------------------------*/
int WaitReadAngle (void){
    unsigned char RxData[8];           // 送信データバッファ [8byte]
    unsigned char CheckSum = 0;        // チェックサム計算用変数
    int           AngleData = 0;       // 角度データ

    // リターンパケット待ち
    while(Serial.read() != 0xFD){}    // Header
    while(Serial.read() != 0xDF){}    // Header
    while(Serial.available() < 8){}   // ID～Sem
    
    for(int i=0; i<=6; i++){
        RxData[i] = Serial.read();
        CheckSum = CheckSum ^ RxData[i];  // ID～DATAまでのXOR
    }

    // チェックサムが一致すれば、角度データ読み出し
    if(CheckSum==Serial.read()){
        AngleData = (int)RxData[6];    // Hi byte
        AngleData = AngleData << 8;
        AngleData |= (int)RxData[5];   // Lo byte
    }

    return AngleData;
}

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