int in0 = 13;   //反転スイッチ=13PIN
int in1 = 12;   //1番線スイッチ=12PIN
int in2 = 11;   //2番線スイッチ=11PIN
int in3 = 10;   //3番線スイッチ=10PIN
int in4 = 9;    //4番線スイッチ=9PIN
int in5 = 8;    //5番線スイッチ=8PIN
int relay = 6;   //リレー6PIN
int point1 = 4;   //ポイント1スイッチ=4PIN
int point2 = 5;   //ポイント2スイッチ=5PIN
int relay2 = 7;   //後進用リレー7PIN
byte point1_servo = 0;   //ポイント1サーボID
byte point2_servo = 0;   //ポイント2サーボID

byte servo_id = 1;			       //サーボID(違う場合は変更)
int rotate_speed = 2000;		       //1周移動時間(秒×100)
int position_offset = 0;                       //全体の位置の調整(+転車台が時計回り、-反時計回り)
boolean toggle_switch = true;                  //反転スイッチがトグルスイッチの場合はtureに変更
int torque_off = 1000;                         //トルクオフの延長時間(ミリ秒)トルクオフにしない場合は0に変更
int punch = 3000;                              //パンチ(最小電流の設定) 30% 0にするとパンチとコンプライアンスの変更はしない
boolean sensor = false;                        //センサーを使う時はtrueに変更
boolean auto_mode = true;                      //自動的に転車台を接続するのをやめる場合はfalseに変更(sensorがfalseの場合は無視)
boolean direction_control = false;             //方向を制御する時はtrueに変更
int sensor_level = 256;                        //センサー感度(センサーの反応に応じて1〜1027で調整、反応しない時は増やす)

boolean reverse = false;
boolean previous_in0 = false;
int previous_position = 1501;			//サーボの以前の位置
int current_line = 0;
int stop_line = 0;
int change_count = 0;
short point_position1 = 0;	//ポイントサーボ1位置
short point_position2 = 0;	//ポイントサーボ2位置

byte torque_on_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x24};	//トルクONコマンド
byte torque_off_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x00, 0x25};	//トルクOFFコマンド
byte position_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x04, 0x01, 0x00, 0x00, 0x64, 0x00, 0x7E};	// ポジションコマンド
byte compliance_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x18, 0x06, 0x01, 0x01, 0x01, 0x02, 0x02, 0xB8, 0x0B, 0x82};    //コンプライアンススロープ2度 パンチ30%
byte point_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 train_control(int mode)
{
  if (direction_control) {
    if (mode == 0) {
      digitalWrite(relay, LOW);
      digitalWrite(relay2, LOW);
    }
    else if (mode == 2) {
      digitalWrite(relay, LOW);
      digitalWrite(relay2, HIGH);
    }
    else {
      digitalWrite(relay, HIGH);
      digitalWrite(relay2, LOW);
    }
  }
  else {
    if (mode == 0) {
      digitalWrite(relay, HIGH);
    }
    else {
     digitalWrite(relay, LOW);
    }
  }
}

void change_point(byte servo_id, short point_position) {
  byte data[10];
  if (torque_off) {
    memcpy(data, torque_on_data, 9);   //トルクONデータ転送
    data[2] = servo_id;                //サーボIDセット
    checksum(data, 9);                 //チェックサム計算
    Serial.write(data,9);             //トルクON
    delay(100);  //0.1秒待つ
  }
     
  memcpy(data, point_data, 10);   //回転データ転送
  data[2] = (byte) servo_id;	      //サーボIDセット
  memcpy(&data[7], &point_position, 2);  //角度設定
  checksum(data, 10);              //チェックサム計算
  Serial.write(data,10);        //変更位置まで回転
  if (torque_off) {
    delay(torque_off);
  
    memcpy(data, torque_off_data, 9);   //トルクOFFデータ転送
    data[2] = servo_id;                //サーボIDセット
    checksum(data, 9);       //チェックサム計算
    Serial.write(data,9);             //トルクOFF
  }
}

void setup() {
  Serial.begin(115200);                 //115200bpsでポートを開く
  delay(500);                            //0.5秒待つ
 // デジタル入力のプルアップ抵抗を有効にする
  pinMode(in1, INPUT_PULLUP);
  pinMode(in2, INPUT_PULLUP);
  pinMode(in0, INPUT_PULLUP);
  pinMode(in3, INPUT_PULLUP);
  pinMode(in4, INPUT_PULLUP);
  pinMode(in5, INPUT_PULLUP);
  pinMode(relay, OUTPUT);
  if (sensor || direction_control) {
    pinMode(relay, OUTPUT);
    if (direction_control) {
      pinMode(relay2, OUTPUT);
    }
  }
  if (point1_servo) {
    pinMode(point1, INPUT_PULLUP);
  }
  if (point2_servo) {
    pinMode(point2, INPUT_PULLUP);
  }
  torque_on_data[2] = servo_id;
  checksum(torque_on_data, 9);           //チェックサム計算
  torque_off_data[2] = servo_id;
  checksum(torque_off_data, 9);          //チェックサム計算
  position_data[2] = servo_id;
  if (punch > 0) {
   compliance_data[2] = servo_id;
   memcpy(&compliance_data[11], &punch, 2);          //パンチ設定
   checksum(compliance_data, 14);         //チェックサム計算
   Serial.write(compliance_data,14);  
    delay(500);  //0.5秒待つ
  }
  if (torque_off == 0) {
    Serial.write(torque_on_data,9);     //サーボトルクON
    if (point1_servo) {
      byte data[9];
      memcpy(data, torque_on_data, 9);   //トルクONデータ転送
      data[2] = point1_servo;                //サーボIDセット
      checksum(data, 9);                 //チェックサム計算
      Serial.write(data,9);             //トルクON
    }
    if (point2_servo) {
      byte data[9];
      memcpy(data, torque_on_data, 9);   //トルクONデータ転送
      data[2] = point2_servo;                //サーボIDセット
      checksum(data, 9);                 //チェックサム計算
      Serial.write(data,9);             //トルクON
    }
    delay(500);                          //0.5秒待つ
  }
}

void loop() {
  
  int current_position;
  int reverse_offset;
  int line;

  if (previous_position > 1500) {
    current_position = 0;
    line = 0;
  }
  else {
    current_position = previous_position;
  }
  if (toggle_switch) {
    reverse = (digitalRead(in0) == LOW);
  }
  else {
    if (digitalRead(in0) == LOW) {
      if (!previous_in0)
        reverse = !reverse;
      previous_in0 = true;
    }
    else
    {
      previous_in0 = false;
    }
  }
  if (sensor) {
    if (analogRead(0) < sensor_level) {
      stop_line = 5;
    }

    if (analogRead(1) < sensor_level) {
      stop_line = 1;
    }

    if (stop_line != 0) {
      if  (stop_line != current_line) {
        train_control(0);  //停止
      }
      else
        stop_line = 0;
    }
  }
   
  if ((digitalRead(in1) == LOW) || (auto_mode && (stop_line == 1))) {
    if (reverse) {
      current_position = -1350;       //停止位置1の反転位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    else {
       current_position = 450;        //停止位置1の位置設定
      reverse_offset = 0;             //逆回転補正値
     }
     line = 1;
  }

  if (digitalRead(in2) == LOW) {
    if (reverse) {
      current_position = -1050;       //停止位置2の反転位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    else {
      current_position = 750;	      //停止位置2の位置設定
      reverse_offset = 0;             //逆回転補正値
     }
    line = 2;   
  }
  if (digitalRead(in3) == LOW) {
    if (reverse) {
      current_position = -900;        //停止位置3の反転位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    else {
      current_position = 900;	      //停止位置3の位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    line = 3;
  }
  
  if (digitalRead(in4) == LOW) {
    if (reverse) {
      current_position = -750;        //停止位置4の反転位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    else {
      current_position = 1050;	      //停止位置4の位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    line = 4;
  }
 
  if ((digitalRead(in5) == LOW) || (auto_mode && (stop_line == 5))) {
   if (reverse) {
      current_position = 1350;        //停止位置5の反転位置設定
      reverse_offset = 0;             //逆回転補正値
    }
    else {
      current_position = -450;	       //停止位置5の位置設定
       reverse_offset = 0;             //逆回転補正値
    }
    line = 5;   
  }
  
  if (previous_position != current_position) {
    byte data[12];
    int move_position;
    if (direction_control) {
        train_control(0);  //停止
    }
    if (torque_off > 0) {
      Serial.write(torque_on_data,9);        //サーボトルクON
      delay(100);                             //0.1秒待つ
    }
   //今回の移動時間を計算
    int move_speed = short(abs((long) previous_position - (long) current_position) * (long) rotate_speed / 3600);
    memcpy(data, position_data, 12);         //回転データ転送
    if (current_position < previous_position) {      
      move_position = current_position + position_offset + reverse_offset;
    }
    else {
      move_position = current_position + position_offset;
    }
    memcpy(data, position_data, 12);         //回転データ転送
    memcpy(&data[7], &move_position, 2);     //変更位置設定
    memcpy(&data[9], &move_speed, 2);        //移動時間設定
    checksum(data, 12);                      //チェックサム計算
    Serial.write(data,12);	             //変更位置まで回転
    if (sensor && (stop_line == 0)) {
      long time = move_speed * 10 + torque_off;
      long t = 0;
      while (t++ < time) {
        if (analogRead(0) < sensor_level) {
          stop_line = 5;
        }

        if (analogRead(1) < sensor_level) {
          stop_line = 1;
        }
        if (stop_line != 0) {
          train_control(0);  //停止
        }
        delayMicroseconds(750);  //ループが1ミリ秒になるように調整
      }
    }
    else {
        delay(move_speed * 10 + torque_off);  //回転終了待つ
    }
    if (torque_off > 0) {
      Serial.write(torque_off_data,9);     //サーボトルクOFF
    }
    int mode = 0;  //停止
    if (direction_control) {
      if (line == 5) {
        change_count = 0;  //前進で突入
        mode = 1;    //正転
      }
      else if (line == 1) {
        change_count = 1;  //後進で突入
        mode = 1;    //正転
      }
      else if (line > 1) {
         change_count++;
        if (change_count%2 == 1) {
          mode = 2;  //反転
        }
        else {
          mode = 1;  //正転
        }
      }
      else {
        mode = 1;  //開始時
      }
    }
    else if (stop_line != 0) {
      if (stop_line == line) {
        mode = 1;  //前進
        stop_line = 0;
      }
    }
    if (mode) {
       train_control(mode);
    }
    current_line = line;
    previous_position = current_position;
  }  
  else  {   
    if (point1_servo) {
      int position1;
      if (digitalRead(point1) == HIGH) {
        position1 = 1000;    //ポイントサーボ1のON位置設定
      }
      else {
        position1 = -1000;     //ポイントサーボ1のOFF位置設定
      }
      if (position1 != point_position1) {  //ポイント1サーボ位置変更
        change_point(point1_servo, position1);
        point_position1 = position1;
      } 
    }  
    if (point2_servo) {
      int position2;
      if (digitalRead(point2) == HIGH) {
        position2 = 1000;    //サーボ2のON位置設定
      }
      else {
        position2 = -1000;     //サーボ2のOFF位置設定
      }
      if (position2 != point_position2) {  //ポイント1サーボ位置変更
        change_point(point2_servo, position2);
        point_position2 = position2;
      }
    }
  }
}