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

#define  VOL1 A0
#define  VOL2 A1
#define  VOL3 A2
#define  AUTO 62
#define  PTS1 63
#define  PTS2 64
#define  PTS3 65
#define  DSWR 66
#define  DSWL 67
#define  EDS1 22
#define  EDS2 24
#define  EDS3 26
#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 13
#define  POUT2 12
#define  POUT3 11
#define  MPWM  7
#define  DIRR  6
#define  DIRL  5

  int pts1;
  int pts2;
  int pts3;
  int dswr;
  int dswl;

void setup() {
  pinMode(VOL1,INPUT);
  pinMode(VOL2,INPUT);  
  pinMode(VOL3,INPUT);
  pinMode(AUTO,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);
  
}

void loop(){
  // 全ての信号を赤にする
    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);

 // スイッチ類のチェック
    pts1 = digitalRead(PTS1);
    pts2 = digitalRead(PTS2);
    pts3 = digitalRead(PTS3);
    dswr = digitalRead(DSWR);
    dswl = digitalRead(DSWL);

  // 出発ホームと到着ホームへの信号を緑にする
    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);
}