#include<Servo.h>
Servo myservo;
int servoPin=11;
int led1 = 10;
int led2 = 9;
int led3 = 8;
int STOPpin = 4;
int CWpin = 3;
int CCWpin =6;
int sc []={180,110,0};


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


void setup(){
  // put your setup code here, to run once:

  Serial.begin(9600);
  pinMode(STOPpin, INPUT_PULLUP);
  pinMode(CCWpin, INPUT_PULLUP);
  pinMode(CWpin, INPUT_PULLUP);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);

  //myservo.attachInterrupt(servoPin);
  myservo.attach(servoPin);
  myservo.write(sc[1]);
  statusText =1;
}

void loop() {
  // put your main code here, to run repeatedly:
  CCWBS = digitalRead(CCWpin);
  SBS = digitalRead(STOPpin);
  CWBS = digitalRead(CWpin);
  if(CCWBS == LOW)
  {
    servoCommand(0);
    int pos = 0;
    digitalWrite(led1, HIGH);
    for(pos=180; pos >=0; pos-=1){
      myservo.write(pos);
      delay(15);
    }
    digitalWrite(led1, LOW);

  }
  else if (SBS == LOW)
  {
    digitalWrite(led2, HIGH);
    servoCommand(1);
    digitalWrite(led2,LOW);
  }
  else if(CWBS ==LOW)
  {
    servoCommand(2);
    int pos =0;
    digitalWrite(led3, HIGH);
    for (pos =0; pos <=180; pos +=1){
      myservo.write(pos);
      delay(15);
    }
    digitalWrite(led3, 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(")");
}