// Sin-Tozan-Unit-05A
// 2021.11/19
// Arduino MEGA 2560 を使用する。
// PWM周波数を31kHzにする。

#define  VOL1 A0
#define  VOL2 A1
#define  VOL3 A2
#define  AUT 62
#define  PTS1 63
#define  PTS2 64
#define  PTS3 65
#define  DSWR 66
#define  DSWL 67
#define  EDS1 22
#define  EDS2 24
#define  EDS3 28
#define  EDS4 32
#define  EDS5 34
#define  TS1  26
#define  TS2  30
#define  TS3  36
#define  J1G  48
#define  S1G  46
#define  J2G  44
#define  S2G  42
#define  J3G  43
#define  S3G  45
#define  J4G  49
#define  S4G  47
#define  J5G  53
#define  S5G  51
#define  J1R  23
#define  S1R  25
#define  J2R  27
#define  S2R  29
#define  J3R  31
#define  S3R  33
#define  J4R  37
#define  S4R  35
#define  J5R  41
#define  S5R  39
#define  POUT1 11
#define  POUT2 12
#define  POUT3 13
#define  MPWM  7
#define  DIRR  5
#define  DIRL  6
  int pts1;
  int pts2;
  int pts3;
  int dswr;
  int dswl;
  int vol1;
  int vol2;
  int vol3;
  int aut;
  int aut1;
  int eds1;
  int eds2;
  int eds3;
  int eds4;
  int eds5;
  int ts1;
  int ts2;
  int ts3;
  int duty;
  int i;

void setup() {
  pinMode(VOL1,INPUT);
  pinMode(VOL2,INPUT);  
  pinMode(VOL3,INPUT);
  pinMode(AUT,INPUT);
  pinMode(PTS1,INPUT);
  pinMode(PTS2,INPUT);
  pinMode(PTS3,INPUT);
  pinMode(DSWR,INPUT);
  pinMode(DSWL,INPUT);
  pinMode(EDS1,INPUT);
  pinMode(EDS2,INPUT);
  pinMode(EDS3,INPUT);
  pinMode(EDS4,INPUT);
  pinMode(EDS5,INPUT);
  pinMode(TS1,INPUT);
  pinMode(TS2,INPUT);
  pinMode(TS3,INPUT);
  
  pinMode(J1G,OUTPUT);
  pinMode(J2G,OUTPUT);
  pinMode(J3G,OUTPUT);
  pinMode(J4G,OUTPUT);
  pinMode(J5G,OUTPUT);
  pinMode(S1G,OUTPUT);
  pinMode(S2G,OUTPUT);
  pinMode(S3G,OUTPUT);
  pinMode(S4G,OUTPUT);
  pinMode(S5G,OUTPUT);
  pinMode(J1R,OUTPUT);
  pinMode(J2R,OUTPUT);
  pinMode(J3R,OUTPUT);
  pinMode(J4R,OUTPUT);
  pinMode(J5R,OUTPUT);
  pinMode(S1R,OUTPUT);
  pinMode(S2R,OUTPUT);
  pinMode(S3R,OUTPUT);
  pinMode(S4R,OUTPUT);
  pinMode(S5R,OUTPUT);

  pinMode(POUT1,OUTPUT);
  pinMode(POUT2,OUTPUT);
  pinMode(POUT3,OUTPUT);
  pinMode(MPWM,OUTPUT);
  pinMode(DIRR,OUTPUT);
  pinMode(DIRL,OUTPUT);

  TCCR4B = (TCCR4B & 0b11111000) | 0x01;   //Timer4を31KHzにする

  digitalWrite(DIRR,LOW);
  digitalWrite(DIRL,LOW);
  analogWrite(MPWM,0);  
  digitalWrite(POUT1,LOW);
  digitalWrite(POUT2,LOW);
  digitalWrite(POUT3,LOW);

  digitalWrite(J1G,LOW);
  digitalWrite(J2G,LOW);
  digitalWrite(J3G,LOW);
  digitalWrite(J4G,LOW);
  digitalWrite(J5G,LOW);
  digitalWrite(S1G,LOW);
  digitalWrite(S2G,LOW);
  digitalWrite(S3G,LOW);
  digitalWrite(S4G,LOW);
  digitalWrite(S5G,LOW);
  digitalWrite(J1R,HIGH);
  digitalWrite(J2R,HIGH);
  digitalWrite(J3R,HIGH);
  digitalWrite(J4R,HIGH);
  digitalWrite(J5R,HIGH);
  digitalWrite(S1R,HIGH);
  digitalWrite(S2R,HIGH);
  digitalWrite(S3R,HIGH);
  digitalWrite(S4R,HIGH);
  digitalWrite(S5R,HIGH);

 duty = 0;
}

void ledhyoji(){
  // 全ての信号を赤にする
    digitalWrite(J1G,LOW);
    digitalWrite(J2G,LOW);
    digitalWrite(J3G,LOW);
    digitalWrite(J4G,LOW);
    digitalWrite(J5G,LOW);
    digitalWrite(S1G,LOW);
    digitalWrite(S2G,LOW);
    digitalWrite(S3G,LOW);
    digitalWrite(S4G,LOW);
    digitalWrite(S5G,LOW);
    digitalWrite(J1R,HIGH);
    digitalWrite(J2R,HIGH);
    digitalWrite(J3R,HIGH);
    digitalWrite(J4R,HIGH);
    digitalWrite(J5R,HIGH);
    digitalWrite(S1R,HIGH);
    digitalWrite(S2R,HIGH);
    digitalWrite(S3R,HIGH);
    digitalWrite(S4R,HIGH);
    digitalWrite(S5R,HIGH);

  // 出発ホームと到着ホームへの信号を緑にする
    if (dswr == HIGH){
      if (pts1 == HIGH){
        if (pts2 == HIGH){
          digitalWrite(J3G,HIGH);
          digitalWrite(S2G,HIGH);
          digitalWrite(J3R,LOW);
          digitalWrite(S2R,LOW);
        } else if (pts3 == HIGH){
          digitalWrite(J4G,HIGH);
          digitalWrite(S2G,HIGH);
          digitalWrite(J4R,LOW);
          digitalWrite(S2R,LOW);
        } else {
          digitalWrite(J5G,HIGH);
          digitalWrite(S2G,HIGH);
          digitalWrite(J5R,LOW);
          digitalWrite(S2R,LOW);
        }
      } else if (pts2 == HIGH){
        digitalWrite(J3G,HIGH);
        digitalWrite(S1G,HIGH);
        digitalWrite(J3R,LOW);
        digitalWrite(S1R,LOW);
      } else if (pts3 == HIGH){
        digitalWrite(J4G,HIGH);
        digitalWrite(S1G,HIGH);
        digitalWrite(J4R,LOW);
        digitalWrite(S1R,LOW);
      } else {
        digitalWrite(J5G,HIGH);
        digitalWrite(S1G,HIGH);
        digitalWrite(J5R,LOW);
        digitalWrite(S1R,LOW);
      }
    } else if (dswl == HIGH){
      if (pts1 == HIGH){
        if (pts2 == HIGH){
          digitalWrite(J2G,HIGH);
          digitalWrite(S3G,HIGH);
          digitalWrite(J2R,LOW);
          digitalWrite(S3R,LOW);
        } else if (pts3 == HIGH){
          digitalWrite(J2G,HIGH);
          digitalWrite(S4G,HIGH);
          digitalWrite(J2R,LOW);
          digitalWrite(S4R,LOW);
        } else {
          digitalWrite(J2G,HIGH);
          digitalWrite(S5G,HIGH);
          digitalWrite(J2R,LOW);
          digitalWrite(S5R,LOW);
        }
      } else if (pts2 == HIGH){
        digitalWrite(J1G,HIGH);
        digitalWrite(S3G,HIGH);
        digitalWrite(J1R,LOW);
        digitalWrite(S3R,LOW);
      } else if (pts3 == HIGH){
        digitalWrite(J1G,HIGH);
        digitalWrite(S4G,HIGH);
        digitalWrite(J1R,LOW);
        digitalWrite(S4R,LOW);
      } else {
        digitalWrite(J1G,HIGH);
        digitalWrite(S5G,HIGH);
        digitalWrite(J1R,LOW);
        digitalWrite(S5R,LOW);
      }
    }
    delay(20);
    return;
}

void mode1(int densha){
  if (analogRead(densha) < 400 ) {   //電車の設定の有無チェック
    return;                          //無の場合はこの運行をパスする
  }
  digitalWrite(POUT1, HIGH);         //経路を設定する
  delay(500);
  digitalWrite(POUT2, HIGH);
  delay(500);
  digitalWrite(DIRL,LOW);         //進行方向を設定する
  digitalWrite(DIRR,HIGH);
  dswr=HIGH;
  dswl=LOW;
  pts1=HIGH;
  pts2=HIGH;
  pts3=LOW;
  ledhyoji();
  analogWrite(MPWM, 0);           //フィーダー出力０
  for (i=0;i<17;i++ ) {           //走行加速開始
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) *(0.1 + 0.025*i ); 
    analogWrite(MPWM, duty);      //フィーダー出力
    delay(200);
  }
 ts2 = digitalRead(TS2);          //場内点通過チェック
  while (ts2 == HIGH ) {           //場内点通過待ち
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) * 0.5;    //通常速度
    analogWrite(MPWM, duty);      //フィーダー出力
    ts2 = digitalRead(TS2);       //場内点通過チェック
    delay(50);
  }
  eds3 = digitalRead(EDS3);       //到着チェック
  while (eds3 == HIGH ) {          //到着待ち
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) * 0.4;    //低速走行
    analogWrite(MPWM, duty);      //フィーダー出力
    eds3 = digitalRead(EDS3);     //到着チェック
    delay(50);
  }
  delay(500);
  analogWrite(MPWM, 0);
  delay(1000);
  return;
}

void mode2(int densha){
  if (analogRead(densha) < 400 ) {   //電車の設定の有無チェック
    return;                          //無の場合はこの運行をパスする
  }
  digitalWrite(POUT1, HIGH);         //経路を設定する
  delay(500);
  digitalWrite(POUT2, LOW);
  delay(500);
  digitalWrite(DIRR,LOW);         //進行方向を設定する
  digitalWrite(DIRL,HIGH);
  dswr=LOW;
  dswl=HIGH;
  pts1=HIGH;
  pts2=LOW;
  pts3=LOW;
  ledhyoji();
  analogWrite(MPWM, 0);           //フィーダー出力０
  for (i=0;i<17;i++ ) {           //走行加速開始
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) *(0.1 + 0.025*i ); 
    analogWrite(MPWM, duty);      //フィーダー出力
    delay(200);
  }
  eds2 = digitalRead(EDS2);       //到着チェック
  while (eds2 == HIGH ) {          //到着待ち
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) * 0.4;    //低速走行
    analogWrite(MPWM, duty);      //フィーダー出力
    eds2 = digitalRead(EDS2);     //到着チェック
    delay(50);
  }
  delay(500);
  analogWrite(MPWM, 0);
  delay(1000);
  return;
}

void mode3(int densha){
  if (analogRead(densha) < 400 ) {   //電車の設定の有無チェック
    return;                          //無の場合はこの運行をパスする
  }
  digitalWrite(POUT1, LOW);         //経路を設定する
  delay(500);
  digitalWrite(POUT2, LOW);
  delay(500);
  digitalWrite(DIRL,LOW);         //進行方向を設定する
  digitalWrite(DIRR,HIGH);
  dswr=HIGH;
  dswl=LOW;
  pts1=LOW;
  pts2=LOW;
  pts3=LOW;
  ledhyoji();
  analogWrite(MPWM, 0);           //フィーダー出力０
  for (i=0;i<17;i++ ) {           //走行加速開始
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) *(0.1 + 0.025*i ); 
    analogWrite(MPWM, duty);      //フィーダー出力
    delay(200);
  }
  eds5 = digitalRead(EDS5);       //到着チェック
  while (eds5 == HIGH ) {          //到着待ち
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) * 0.4;    //低速走行
    analogWrite(MPWM, duty);      //フィーダー出力
    eds5 = digitalRead(EDS5);     //到着チェック
    delay(50);
  }
  delay(500);
  analogWrite(MPWM, 0);
  delay(1000);
  return;
}

void mode4(int densha){
  if (analogRead(densha) < 400 ) {   //電車の設定の有無チェック
    return;                          //無の場合はこの運行をパスする
  }
  digitalWrite(POUT1, LOW);         //経路を設定する
  delay(500);
  digitalWrite(POUT2, HIGH);
  delay(500);
  digitalWrite(DIRR,LOW);         //進行方向を設定する
  digitalWrite(DIRL,HIGH);
  dswr=LOW;
  dswl=HIGH;
  pts1=LOW;
  pts2=HIGH;
  pts3=LOW;
  ledhyoji();
  analogWrite(MPWM, 0);           //フィーダー出力０
  for (i=0;i<17;i++ ) {           //走行加速開始
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) *(0.1 + 0.025*i ); 
    analogWrite(MPWM, duty);      //フィーダー出力
    delay(200);
  }
 ts1 = digitalRead(TS1);          //場内点通過チェック
  while (ts1 == HIGH ) {           //場内点通過待ち
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) * 0.5;    //通常速度
    analogWrite(MPWM, duty);      //フィーダー出力
    ts1 = digitalRead(TS1);       //場内点通過チェック
    delay(50);
  }
  eds1 = digitalRead(EDS1);       //到着チェック
  while (eds1 == HIGH ) {          //到着待ち
    duty = analogRead(densha);    //VRの値を読込む
    duty = (duty - 500) * 0.4;    //低速走行
    analogWrite(MPWM, duty);      //フィーダー出力
    eds1 = digitalRead(EDS1);     //到着チェック
    delay(50);
  }
  delay(500);
  analogWrite(MPWM, 0);
  delay(1000);
  return;
}

void loop(){
  aut = digitalRead(AUT);     //自動運転スイッチをチェック
  
  if (aut==LOW){     //手動運転モード
      pts1 = digitalRead(PTS1);   //スイッチ類のチェック
      pts2 = digitalRead(PTS2);
      dswr = digitalRead(DSWR);
      dswl = digitalRead(DSWL);
      vol1 = analogRead(VOL1);
      ledhyoji();                     //パネル表示
      
      if (pts1==HIGH){                //ポイント操作
        digitalWrite(POUT1,HIGH);
        delay(100);
      } else {
        digitalWrite(POUT1,LOW);
        delay(100);
      }
      if (pts2==HIGH){
        digitalWrite(POUT2,HIGH);
        delay(100);
      } else {
        digitalWrite(POUT2,LOW);
        delay(100);
      }
     
      if (dswr==HIGH){     //進行方向設定
        digitalWrite(DIRR,HIGH);
      }  else {
        digitalWrite(DIRR,LOW);
      }
      if (dswl==HIGH){
        digitalWrite(DIRL,HIGH);
      } else {
        digitalWrite(DIRL,LOW);
      }
      
      duty=(vol1-500)/2;
      analogWrite(MPWM,duty);  
      
  } else if (aut==HIGH){   //自動運転モード
      analogWrite(MPWM, 0);   //暴走防止
      mode3(0);               //densHa0の運行
      mode2(0);
      mode1(0);
      mode4(0);
  }
  delay(10);
}