//Cliff Aaker
//2019.05.25
//Use servo pin 7 for center only
#include <Servo.h>

Servo servoA;  // create servo object to control a servo pin2
int toggleApin = 8; //create toggle switch pin
int lasttogAstate = 3; // 0 = LOW 1 = High
int curtogAstate = 0; // 0 = LOW 1 = High
Servo servoB;  // create servo object to control a servo pin3
int toggleBpin = 9; //create toggle switch pin
int lasttogBstate = 3; // 0 = LOW 1 = High
int curtogBstate = 0; // 0 = LOW 1 = High
Servo servoC;  // create servo object to control a servo pin4
int toggleCpin = 10; //create toggle switch pin
int lasttogCstate = 3; // 0 = LOW 1 = High
int curtogCstate = 0; // 0 = LOW 1 = High
Servo servoD;  // create servo object to control a servo pin5
int toggleDpin = 11; //create toggle switch pin
int lasttogDstate = 3; // 0 = LOW 1 = High
int curtogDstate = 0; // 0 = LOW 1 = High
Servo servoCTR;  // create servo object to control a servo pin7

int pos = 30;    // variable to store the servo position

void setup() 
{
  pinMode(toggleApin, INPUT_PULLUP);  
  pinMode(toggleBpin, INPUT_PULLUP); 
  pinMode(toggleCpin, INPUT_PULLUP);  
  pinMode(toggleDpin, INPUT_PULLUP); 
  
  servoCTR.attach(7);  // attaches the servo (pin) to the servo object
}

void loop() 
{
 //BEGIN SERVO A
  if (digitalRead(toggleApin) == LOW && digitalRead(toggleApin) == LOW && digitalRead(toggleApin) == LOW) 
  { curtogAstate = 0; } else { curtogAstate = 1; }
  
  if (curtogAstate != lasttogAstate) {   lasttogAstate = curtogAstate ; servoA.attach(2);
     if (curtogAstate == 0) {
     for (pos = 40; pos <= 160; pos += 1) { // in steps of 1 degree
     servoA.write(pos);              // tell servo to go to position in variable 'pos'
     delay(15);}                      // waits 15ms for the servo to reach the position    
   } else { 
     for (pos = 160; pos >= 40; pos -= 1) { // in steps of 1 degree
     servoA.write(pos);              // tell servo to go to position in variable 'pos'
     delay(15);}                       // waits 15ms for the servo to reach the position
   }  // else
   servoA.detach();
 }  //curtog
 //END SERVO A

 //BEGIN SERVO B 
  if (digitalRead(toggleBpin) == LOW && digitalRead(toggleBpin) == LOW && digitalRead(toggleBpin) == LOW)
  { curtogBstate = 0; } else { curtogBstate = 1; }
  
  if (curtogBstate != lasttogBstate) {   lasttogBstate = curtogBstate ; servoB.attach(3);
    if (curtogBstate == 0) {
      for (pos = 40; pos <= 160; pos += 1) { // in steps of 1 degree
      servoB.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);}                      // waits 15ms for the servo to reach the position
    } else { 
      for (pos = 160; pos >= 40; pos -= 1) { // in steps of 1 degree
      servoB.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);}                       // waits 15ms for the servo to reach the position
   }  // else
   servoB.detach();
 }  //curtog
// END SERVO B

 //BEGIN SERVO C
  if (digitalRead(toggleCpin) == LOW && digitalRead(toggleCpin) == LOW && digitalRead(toggleCpin) == LOW) 
  { curtogCstate = 0; } else { curtogCstate = 1; }
  
  if (curtogCstate != lasttogCstate) {   lasttogCstate = curtogCstate ; servoC.attach(4);
     if (curtogCstate == 0) {
     for (pos = 40; pos <= 160; pos += 1) { // in steps of 1 degree
     servoC.write(pos);              // tell servo to go to position in variable 'pos'
     delay(15);}                      // waits 15ms for the servo to reach the position    
   } else { 
     for (pos = 160; pos >= 40; pos -= 1) { // in steps of 1 degree
     servoC.write(pos);              // tell servo to go to position in variable 'pos'
     delay(15);}                       // waits 15ms for the servo to reach the position
   }  // else
   servoC.detach();
 }  //curtog
 //END SERVO C

 //BEGIN SERVO D 
  if (digitalRead(toggleDpin) == LOW && digitalRead(toggleDpin) == LOW && digitalRead(toggleDpin) == LOW)
  { curtogDstate = 0; } else { curtogDstate = 1; }
  
  if (curtogDstate != lasttogDstate) {   lasttogDstate = curtogDstate ; servoD.attach(5);
    if (curtogDstate == 0) {
      for (pos = 40; pos <= 160; pos += 1) { // in steps of 1 degree
      servoD.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);}                      // waits 15ms for the servo to reach the position
    } else { 
      for (pos = 160; pos >= 40; pos -= 1) { // in steps of 1 degree
      servoD.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);}                       // waits 15ms for the servo to reach the position
   }  // else
   servoD.detach();
 }  //curtog
// END SERVO D
// BEGIN SERVO CTR
 servoCTR.write(90);
//END SERVO CTR

}  //END VOID LOOP