/**
 * @file: servo control example
 *
 * @description
 * drive forward, drive backward
 * 1 servo, LiPo battery
 *
 * The minimum (backward) and maximum (forward) values
 * will be different depending on your specific servo motor.
 * Ideally, it should be between 1 and 2 milliseconds, but in practice,
 * 0.5 - 2.5 milliseconds works well.
 * Try different values to see what numbers are best for you.
 *
 */

// include additional headers
#include <Servo.h>

 //global declarations
#define SERVO 4        // define a pin for servo
#define BUTTON 2       // define pin for button
#define LED 13
Servo myservo;         // initialize  servo

// played around with values that sets the servos to each position
// these values need to be set for each servo!!!
const int neutral = 1500;
const int forward = 2000;
const int backward = 900;
int buttonState = 0;
int servoState = 0;

//--- Function: Setup ()
void setup() {
  pinMode(BUTTON, INPUT);
  pinMode(SERVO, OUTPUT);	// want servo pin to be an output
  pinMode(LED, OUTPUT);
  myservo.attach(SERVO);	// attach pin to the servo
  myservo.writeMicroseconds(neutral);  // set servo to mid-point
  digitalWrite(LED, LOW);
}

//--- Function: loop ()
void loop() {
    buttonState = digitalRead(BUTTON);

    if ((buttonState==1) && (servoState==0)) {
      digitalWrite(LED, HIGH);
      myservo.writeMicroseconds(backward);
      delay(200);
      //myservo.writeMicroseconds(neutral);
      servoState = 1;
    }
    else if ((buttonState==0) && (servoState==1)) {
      //myservo.writeMicroseconds(forward);
      //delay(200);
      myservo.writeMicroseconds(neutral);
      delay(200);
      servoState = 0;
      digitalWrite(LED, LOW);
    }
    else {
      digitalWrite(LED, servoState ? HIGH : LOW);
      myservo.writeMicroseconds(servoState ? backward : neutral);
    }
}