int in1 = 8;   //ポイント1スイッチ=8PIN
int in2 = 9;   //ポイント2スイッチ=9PIN
	
short previous_position[] = {0 , 0};	//サーボ以前の位置

//トルクONコマンド
byte torque_on[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x24};
// ポジションコマンド
byte position_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x02, 0x01, 0x00, 0x00, 0x1C};

void memcpy(byte* buf, byte* data, int n)     // データ転送
{
  int sum = 0;
  for(int i = 0; i < n; i++){
    buf[i] = data[i];
  }
}

void checksum(byte* data, int n)     // チェックサムの設定
{
  int sum = 0;
  for(int i = 2; i < n - 1; i++){
    sum = sum ^ data[i];
  }
  data[ n-1 ] = sum;
}

void setup() {
  byte data[9];
  // デジタル入力のプルアップ抵抗を有効にする
  pinMode(in1, INPUT_PULLUP);
  pinMode(in2, INPUT_PULLUP);
  Serial.begin(115200);                  //115200bpsでポートを開く
  for (int i = 0; i < 2; i++) {
    delay(500);                            //0.5秒待つ
    memcpy(data, torque_on, 9);	          //トルクOKデータ転送
    data[2] = (byte) (i + 1);             //サーボIDセット
    checksum(data, 9);			  //チェックサム計算
    Serial.write(data,9);                //サーボ2トルクON
  }
}

void loop() {
  short current_position[2];

  if (digitalRead(in1) == HIGH) {
    current_position[0] = 1000;    //サーボ1のON位置設定
  }
  else {
    current_position[0] = -1000;     //サーボ1のOFF位置設定
  }
  if (digitalRead(in2) == HIGH) {
    current_position[1] = 1000;    //サーボ2のON位置設定
  }
  else {
    current_position[1] = -1000;     //サーボ2のOFF位置設定
  }

  for (int i = 0;i < 2;i++) {
    if (previous_position[i] != current_position[i]) {  //サーボ位置変更
      byte data[10];
 
      memcpy(data, position_data, 10);   //回転データ転送
      data[2] = (byte) (i + 1);	      //サーボIDセット
      memcpy(&data[7], &current_position[i], 2);  //角度設定
      checksum(data, 10);              //チェックサム計算
      Serial.write(data,10);			 //変更位置まで回転
      previous_position[i] = current_position[i];
    }
  }
  
  delay(100); //スイッチチェック間隔
}