// Written by R. Jordan Kreindler, September 2016
// Servo moves to 0-40-80-120-160 degree positions and 
// different colored LEDs light when servo at these positions

#include <Servo.h>

Servo servo1;  // create a servo1 servo object

const int   servoPin  =    8; // Pin assignment for servo data
const int   redPin    =    9; // Pin assignment for red LED
const int   yellowPin =   10; // Pin assignment for yellow LED
const int   greenPin  =   11; // Pin assignment for green LED
const int   bluePin   =   12; // Pin assignment for blue LED
const int   whitePin  =   13; // Pin assignment for white LED

int         position  =    0; // Store a value in position
int         delay1    =  500; // Delay before starting sweep
int         delay2    = 1000; // Delay before moving to next position

void setup() {
  servo1.attach(servoPin);    // Attach the servo object to the servo Pin
  pinMode(redPin,    OUTPUT); // Set red LED pin to receive output from Arduino
  pinMode(yellowPin, OUTPUT); // Set yellow LED pin to receive output from Arduino
  pinMode(greenPin,  OUTPUT); // Set green LED pin to receive output from Arduino
  pinMode(bluePin, OUTPUT);   // Set blue LED pin to receive output from Arduino
  pinMode(whitePin, OUTPUT);  // Set white LED pin to receive output from Arduino
}

void loop() {
  // Move the servo to 40-80-120-160 degree positions
  // Pause at these positions for delay2/1000 second(s)
  // Red at 0-degrees, Yellow at 40-degrees, Green at 80-degrees, Blue at 120-degrees
  delay(delay1); // Delay before starting sweep
  for (position = 40; position <= 160; position = position+40) {
    servo1.write(position);
    if (position == 40) {
      digitalWrite(yellowPin, HIGH);
      delay(delay2);
      digitalWrite(yellowPin, LOW);
    }
    if (position == 80) {
      digitalWrite(greenPin, HIGH);
      delay(delay2);
      digitalWrite(greenPin, LOW);
    }
    if (position == 120) {
      digitalWrite(bluePin, HIGH);
      delay(delay2);
      digitalWrite(bluePin, LOW);
    }
    if (position == 160) {
      digitalWrite(whitePin, HIGH);
      delay(delay2);
      digitalWrite(whitePin, LOW);
    }
  }
  // Reverse Servo Direction 120-80-40-0 degree positions
  // Pause at these positions for delay2/1000 seconds
  // Light the appropriate LED at 0-degrees Red, at 40-degrees Yellow, 
  // at 80 degrees Green
  delay(delay1); // Delay before starting sweep
  for (position = 120; position >=0; position = position-40) {
    servo1.write(position);
    if (position == 0) {
      digitalWrite(redPin, HIGH);
      delay(delay2);
      digitalWrite(redPin, LOW);
    }
    if (position == 40) {
      digitalWrite(yellowPin, HIGH);
      delay(delay2);
      digitalWrite(yellowPin, LOW);
    }
    if (position == 80) {
      digitalWrite(greenPin, HIGH);
      delay(delay2);
      digitalWrite(greenPin, LOW);
    }
    if (position == 120) {
      digitalWrite(bluePin, HIGH);
      delay(delay2);
      digitalWrite(bluePin, LOW);
    }
  }
}