#include <Servo.h>
Servo myservo;
int servoPin=9;

int STOPpin = 4;
int CWpin = 3;
int CCWpin =5;
int sc []={180,110,0};

int StopLight=8;
int CWLight=7;
int CCWLight=6;

String scText[] = {"CCW","STOP","CW"};
int statusText;
int CWBS, CCWBS, SBS;


void setup() {
  // put your setup code here, to run once:
  //int STOPpin = 4;
  int CWpin = 5;
  int CCWpin = 3;
  Serial.begin(9600);
  pinMode(STOPpin, INPUT_PULLUP);
  pinMode(CCWpin, INPUT_PULLUP);
  pinMode(CWpin, INPUT_PULLUP);
  myservo.attach(servoPin);
  myservo.write(sc[1]);
  statusText =1;
}

void loop() {
  // put your main code here, to run repeatedly:
  int STOPpin = 4;
  int CWpin = 5;
  int CCWpin = 3;
  CCWBS = digitalRead(CCWpin);
  SBS = digitalRead(STOPpin);
  CWBS = digitalRead(CWpin);
  if(CCWBS == LOW)
  {
    servoCommand(0);
    int pos = 0;
    digitalWrite(CCWLight, HIGH);
    for(pos=180; pos >=0; pos-=1){
      myservo.write(pos);
      delay(15);
    }
    digitalWrite(CCWLight, LOW);
  }
  else if (SBS == LOW)
  {
    digitalWrite(StopLight, HIGH);
    servoCommand(1);
    digitalWrite(StopLight, LOW);
  }
  else if(CWBS ==LOW)
  {
    servoCommand(2);
    int pos =0;
    digitalWrite(CWLight, HIGH);
    for (pos =0; pos <=180; pos +=1){
      myservo.write(pos);
      delay(15);
    }
    digitalWrite(CWLight, LOW);
  }
  Serial.println(scText[statusText]);
  delay(50);
}

void servoCommand(int n)
{
  statusText = n;
  myservo.write(sc[n]);
  Serial.print("Going to..");
  Serial.print(scText[n]);
  Serial.print("(");
  Serial.print(sc[n]);
  Serial.print(")");
}