#include <Servo.h>

int pwm = A0;
int pushbutton1 = 8;
int pushbutton2 = 9;
int pushbutton3 = 10;
int pushbutton4 = 11;
int angle = 90;

Servo myServo;

enum state 
{
  idle,
  sweeping,
  zero,
  nighty,
  oneeighty
};

state currentstate = idle; 

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  myServo.attach(pwm);
  myServo.write(angle);

  pinMode(pushbutton1, INPUT_PULLUP);
  pinMode(pushbutton2, INPUT_PULLUP);
  pinMode(pushbutton3, INPUT_PULLUP);
  pinMode(pushbutton4, INPUT_PULLUP);



}

void loop() {
    if(digitalRead(pushbutton1) == LOW)
    {
      delay(20);
      while(digitalRead(pushbutton1) == LOW); 
      Serial.println("pushed.");
      delay(20);
      currentstate = sweeping; 
    }
    else if(digitalRead(pushbutton2) == LOW)
    {
      delay(20);
      while(digitalRead(pushbutton2) == LOW);
      Serial.println("pushed0.");
      delay(20);
      currentstate = zero;
    }
    else if(digitalRead(pushbutton3) == LOW)
    {
      delay(20);
      while(digitalRead(pushbutton3) == LOW);
      Serial.println("pushed90.");
      delay(20);
      currentstate = nighty;
    }
    else if(digitalRead(pushbutton4) == LOW)
    {
      delay(20);
      while(digitalRead(pushbutton4) == LOW);
      Serial.println("pushed180.");
      delay(20);
      currentstate = oneeighty;
    }

    switch(currentstate){
      case sweeping:
          sweep();
          currentstate = idle;
          break;
      case zero:
          moveTo0();
          currentstate = idle;
          break;
      case nighty:
          moveTo90();
          currentstate = idle;
          break;
      case oneeighty:
          moveTo180();
          currentstate = idle;
          break;
      case idle:
      default:
          break;
    }
}

void sweep()
{
    int initialAngle = angle;

    // 从当前角度扫到180
    while(angle < 180) {
      Serial.println(angle);
      myServo.write(angle);
      delay(5);
      angle++;
    }

    // 从180扫回到0
    while(angle > 0) {
      Serial.println(angle);
      myServo.write(angle);
      delay(5);
      angle--;
    }

    // 从0扫回initialAngle（90）
    while(angle < initialAngle) {
      Serial.println(angle);
      myServo.write(angle);
      delay(5);
      angle++;
    }    
}

void moveTo0()
{
    myServo.write(0);
    delay(5);
}
void moveTo90()
{
    myServo.write(90);
    delay(5);
}
void moveTo180()
{
    myServo.write(180);
    delay(5);
}