nwmedia-c262-lab8.ino

#include <Servo.h>

#define PIN_SERVO_FRONT 9
#define PIN_SERVO_BACK 10
#define PIN_POT A0

Servo servoFront;
Servo servoBack;

enum State { REST, FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT };

State state = REST;

void setup() {
  Serial.begin(9600);
  servoFront.attach(PIN_SERVO_FRONT);
  servoBack.attach(PIN_SERVO_BACK);
  servoFront.write(90);
  servoBack.write(90);
  pinMode(PIN_POT, INPUT);
}

void loop() {
  int speed = analogRead(PIN_POT);
  if (speed < 24) {
    state = REST;
  } else if (state == REST) {
    state = FRONT_LEFT;
  }
  switch (state) {
    case FRONT_LEFT:
      servoFront.write(45);
      delay(1024 - speed);
      state = BACK_RIGHT;
      break;
    case BACK_RIGHT:
      servoBack.write(135);
      delay(1024 - speed);
      state = FRONT_RIGHT;
      break;
    case FRONT_RIGHT:
      servoFront.write(135);
      delay(1024 - speed);
      state = BACK_LEFT;
      break;
    case BACK_LEFT:
      servoBack.write(45);
      delay(1024 - speed);
      state = FRONT_LEFT;
      break;
    case REST:
      servoFront.write(90);
      servoBack.write(90);
      break;
  }
}