#include <EEPROM.h>
#include <Servo.h>

#define SW_p1 22
#define SW_p2 24
#define SW_p3 26
#define SW_p4 28
#define SW_p5 30

#define LED_1 44    //偶数番号スイッチLED
#define LED_11 45  //奇数番号番線表示LED
#define LED_2 46
#define LED_22 47
#define LED_3 48
#define LED_33 49
#define LED_4 50
#define LED_44 51
#define LED_5 52
#define LED_55 53
#define CHIEN 41

Servo servo_p1;
Servo servo_p2;
Servo servo_p3;
Servo servo_p4;

int pos;
int servo_p1EEP;
int servo_p2EEP;
int servo_p3EEP;
int servo_p4EEP;
int angle_1;
int angle_2;
int angle_3;
int angle_4;
int servo_p1R = 74;
int servo_p1L = 89;
int servo_p2R = 79;
int servo_p2L = 95;    //元数値94
int servo_p3R = 81;    //元数値82
int servo_p3L = 98;    //元数値97
int servo_p4R = 75;
int servo_p4L = 92;    //元数値90

void setup()
{
pinMode(SW_p1,INPUT);
pinMode(SW_p2,INPUT);
pinMode(SW_p3,INPUT);
pinMode(SW_p4,INPUT);
pinMode(SW_p5,INPUT);
pinMode(23,OUTPUT);
pinMode(25,OUTPUT);
pinMode(27,OUTPUT);
pinMode(29,OUTPUT);
pinMode(LED_1, OUTPUT);
pinMode(LED_2, OUTPUT);
pinMode(LED_3, OUTPUT);
pinMode(LED_4, OUTPUT);
pinMode(LED_5, OUTPUT);
pinMode(LED_11, OUTPUT);
pinMode(LED_22, OUTPUT);
pinMode(LED_33, OUTPUT);
pinMode(LED_44, OUTPUT);
pinMode(LED_55, OUTPUT);
pinMode(CHIEN, OUTPUT);

servo_p1.attach(23);
servo_p2.attach(25);
servo_p3.attach(27);
servo_p4.attach(29);

digitalWrite(CHIEN,HIGH);

servo_p1EEP = EEPROM.read(1);
servo_p2EEP = EEPROM.read(2);
servo_p3EEP = EEPROM.read(3);
servo_p4EEP = EEPROM.read(4);

servo_p1.write(servo_p1EEP);
servo_p2.write(servo_p2EEP);
servo_p3.write(servo_p3EEP);
servo_p4.write(servo_p4EEP);


if((servo_p1EEP == servo_p1L) && (servo_p2EEP == servo_p2L) && (servo_p3EEP == servo_p3L ) && (servo_p4EEP == servo_p4L)){
digitalWrite(LED_5,HIGH);
digitalWrite(LED_55,HIGH);
}

if((servo_p1EEP == servo_p1L) && (servo_p2EEP == servo_p2L) && (servo_p3EEP == servo_p3L ) && (servo_p4EEP == servo_p4R)){
digitalWrite(LED_4,HIGH);
digitalWrite(LED_44,HIGH);
}

if((servo_p1EEP == servo_p1L) && (servo_p2EEP == servo_p2L) && (servo_p3EEP == servo_p3R
 )){
digitalWrite(LED_3,HIGH);
digitalWrite(LED_33,HIGH);
}

if((servo_p1EEP == servo_p1L) && (servo_p2EEP == servo_p2R)){
digitalWrite(LED_2,HIGH);
digitalWrite(LED_22,HIGH);
}

if((servo_p1EEP == servo_p1R)){
digitalWrite(LED_1,HIGH);
digitalWrite(LED_11,HIGH);
}

}

void loop()
{

angle_1 = servo_p1.read();
angle_2 = servo_p2.read();
angle_3 = servo_p3.read();
angle_4 = servo_p4.read();

if(angle_1 != EEPROM.read(1))
{
EEPROM.write(1,angle_1);
}

if(angle_2 != EEPROM.read(2))
{
EEPROM.write(2,angle_2);
}

if(angle_3 != EEPROM.read(3))
{
EEPROM.write(3,angle_3);
}

if(angle_4 != EEPROM.read(4))
{
EEPROM.write(4,angle_4);
}

//5番線開通 SW(5)ON：ポイント(A)右・ポイント(B)右・ポイント(C)右・ポイント(D)右
//＝サーボ(A)左・サーボ(B)左・サーボ(C)左・サーボ(D)左

if(digitalRead(SW_p5)==HIGH){

digitalWrite(LED_1,LOW);
digitalWrite(LED_11,LOW);
digitalWrite(LED_2,LOW);
digitalWrite(LED_22,LOW);
digitalWrite(LED_3,LOW);
digitalWrite(LED_33,LOW);
digitalWrite(LED_4,LOW);
digitalWrite(LED_44,LOW);
digitalWrite(LED_5,HIGH);      //5番線LED点灯
digitalWrite(LED_55,LOW);

if(angle_1==servo_p1R)
for(pos = servo_p1R; pos<=servo_p1L; pos+=1)
{
servo_p1.write(pos);
delay(60);
}

if(angle_2==servo_p2R)
for(pos = servo_p2R; pos<=servo_p2L; pos+=1)
{
servo_p2.write(pos);
delay(60);
}

if(angle_3==servo_p3R)
for(pos = servo_p3R; pos<=servo_p3L; pos+=1)
{
servo_p3.write(pos);
delay(60);
}

if(angle_4==servo_p4R)
for(pos = servo_p4R; pos<=servo_p4L; pos+=1)
{
servo_p4.write(pos);
delay(60);
}
digitalWrite(LED_55,HIGH);
}

//4番線開通 SW(4)ON：ポイント(A)右・ポイント(B)右・ポイント(C)右・ポイント(D)左
//＝サーボ(A)左・サーボ(B)左・サーボ(C)左・サーボ(D)右

if(digitalRead(SW_p4)==HIGH){

digitalWrite(LED_1,LOW);
digitalWrite(LED_11,LOW);
digitalWrite(LED_2,LOW);
digitalWrite(LED_22,LOW);
digitalWrite(LED_3,LOW);
digitalWrite(LED_33,LOW);
digitalWrite(LED_5,LOW);
digitalWrite(LED_55,LOW);
digitalWrite(LED_4,HIGH);
digitalWrite(LED_44,LOW);      //4番線LED点灯

if(angle_1==servo_p1R)
for(pos = servo_p1R; pos<=servo_p1L; pos+=1)
{
servo_p1.write(pos);
delay(60);
}

if(angle_2==servo_p2R)
for(pos = servo_p2R; pos<=servo_p2L; pos+=1)
{
servo_p2.write(pos);
delay(60);
}

if(angle_3==servo_p3R)
for(pos = servo_p3R; pos<=servo_p3L; pos+=1)
{
servo_p3.write(pos);
delay(60);
}

if(angle_4==servo_p4L)
for(pos = servo_p4L; pos>=servo_p4R; pos-=1)
{
servo_p4.write(pos);
delay(60);
}
digitalWrite(LED_44,HIGH);
}

//3番線開通 SW(3)ON：ポイント(A)右・ポイント(B)右・ポイント(C)左
//＝サーボ(A)左・サーボ(B)左・サーボ(C)右

if(digitalRead(SW_p3)==HIGH){

digitalWrite(LED_1,LOW);
digitalWrite(LED_11,LOW);
digitalWrite(LED_2,LOW);
digitalWrite(LED_22,LOW);
digitalWrite(LED_4,LOW);
digitalWrite(LED_44,LOW);
digitalWrite(LED_5,LOW);
digitalWrite(LED_55,LOW);
digitalWrite(LED_3,HIGH);      //３番線LED点灯
digitalWrite(LED_33,LOW);

if(angle_1==servo_p1R)
for(pos = servo_p1R; pos<=servo_p1L; pos+=1)
{
servo_p1.write(pos);
delay(60);
}

if(angle_2==servo_p2R)
for(pos = servo_p2R; pos<=servo_p2L; pos+=1)
{
servo_p2.write(pos);
delay(60);
}

if(angle_3==servo_p3L)
for(pos = servo_p3L; pos>=servo_p3R; pos-=1)
{
servo_p3.write(pos);
delay(60);
}
digitalWrite(LED_33,HIGH);
}

//2番線開通 SW(2)ON：ポイント(A)右・ポイント(B)左
//＝サーボ(A)左・サーボ(B)右

if(digitalRead(SW_p2)==HIGH){

digitalWrite(LED_1,LOW);
digitalWrite(LED_11,LOW);
digitalWrite(LED_3,LOW);
digitalWrite(LED_33,LOW);
digitalWrite(LED_4,LOW);
digitalWrite(LED_44,LOW);
digitalWrite(LED_5,LOW);
digitalWrite(LED_55,LOW);
digitalWrite(LED_2,HIGH);      //2番線LED点灯
digitalWrite(LED_22,LOW);

if(angle_1==servo_p1R)
for(pos = servo_p1R; pos<=servo_p1L; pos+=1)
{
servo_p1.write(pos);
delay(60);
}

if(angle_2==servo_p2L)
for(pos = servo_p2L; pos>=servo_p2R; pos-=1)
{
servo_p2.write(pos);
delay(60);
}
digitalWrite(LED_22,HIGH);
}

//1番線開通 SW(1)ON：サーボ(A)左
//＝サーボ(A)右

if(digitalRead(SW_p1)==HIGH){

digitalWrite(LED_2,LOW);
digitalWrite(LED_22,LOW);
digitalWrite(LED_3,LOW);
digitalWrite(LED_33,LOW);
digitalWrite(LED_4,LOW);
digitalWrite(LED_44,LOW);
digitalWrite(LED_5,LOW);
digitalWrite(LED_55,LOW);
digitalWrite(LED_1,HIGH);        //1番線LED点灯
digitalWrite(LED_11,LOW);

if(angle_1==servo_p1L)
for(pos = servo_p1L; pos>=servo_p1R; pos-=1)
{
servo_p1.write(pos);
delay(60);
}
digitalWrite(LED_11,HIGH);
}
}