int in1 = 8;   //ポイント1スイッチ=8PIN

short previous_position = 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() {
  // デジタル入力のプルアップ抵抗を有効にする
  pinMode(in1, INPUT_PULLUP);
  Serial.begin(115200);                  //115200bpsでポートを開く
  delay(500);                             //0.5秒待つ
  Serial.write(torque_on,9);             //トルクON
}

void loop() {
  short current_position;

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

  if (previous_position != current_position) {  //サーボ位置変更
	byte data[10];

    memcpy(data, position_data, 10);   //回転データ転送
    memcpy(&data[7], &current_position, 2);  //角度設定
    checksum(data, 10);              //チェックサム計算
    Serial.write(data,10);			 //変更位置まで回転
    previous_position = current_position;
  }
  delay(100); //スイッチチェック間隔
}