train code

#include <Servo.h>

Servo servo0;

//don't change
const int LDR0 = 0;
const int LDR1 = 1;
const int USS0 = 2;

//pinmodes
const int greenPin = 13;
const int yellowPin = 12;
const int redPin = 11;
const int motorPin = 10;
const int trigPin = 8;
const int echoPin = 7;
const int buttonPin = 2;

//change
int WayPoint = 0;
int hdrLoop = 0;
int LDRValue0 = 0;
int LDRValue1 = 0;
int light_sensitivity0 = 150;
int light_sensitivity1 = 200;
int ultra_sensitivity = 4;
int pos = 0;
long duration, distance;


void setup()
{
  Serial.begin(9600);
  pinMode(greenPin, OUTPUT); //green
  digitalWrite(greenPin, HIGH);
  pinMode(yellowPin, OUTPUT); //yellow
  digitalWrite(yellowPin, LOW);
  pinMode(redPin, OUTPUT); //red
  digitalWrite(redPin, LOW);
  pinMode(motorPin, OUTPUT); //motor
  digitalWrite(motorPin, LOW);
  pinMode(trigPin, OUTPUT); //trigPin
  digitalWrite(trigPin, LOW);
  pinMode(echoPin, INPUT); //echoPin
  digitalWrite(echoPin, LOW);
  pinMode(buttonPin, INPUT); //button
  digitalWrite(buttonPin, LOW);

  servo0.attach(9);
}

//clockwise
void servoCW(int deg)
{
  for(pos = 0; pos <= deg; pos += 1) // goes from 0 degrees to n degrees
  {                                  // in steps of 1 degree
    servo0.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}

//counter clock-wise
void servoCCW(int deg)
{
  for(pos = 0; pos >= deg; pos -= 1) // goes from 0 degrees to n degrees
  {                                  // in steps of 1 degree
    servo0.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}

void loop()
{
  delay(50);

  //button is pressed
  if(digitalRead(buttonPin) == HIGH)
  {
    digitalWrite(13, HIGH);
    digitalWrite(12, LOW);
    digitalWrite(11, LOW);
  }

  //if nothing is on tracks
  if (hdrLoop >= 20)
  {
   servoCW(180);
  }

  //check train tracks
  if (WayPoint == 3 && hdrLoop < 20)
  {
    digitalWrite(11, HIGH);

    //http://www.instructables.com/id/Simple-Arduino-and-HC-SR04-Example/step3/Upload-the-sketch/
    digitalWrite(8, LOW);
    delayMicroseconds(2);
    digitalWrite(8, HIGH);
    delayMicroseconds(10);
    digitalWrite(8, LOW);
    duration = pulseIn(7, HIGH);
    distance = (duration/2) / 29.1;

    if(distance <= 8.4)
    {
      hdrLoop = 0;
    }
    hdrLoop += 1;
    delay(100);
  }

  //pass second sensor
  if(WayPoint == 2)
  {
    LDRValue1 = analogRead(LDR1);
    if(LDRValue1 < light_sensitivity1)
    {
      WayPoint += 1;
      digitalWrite(13, LOW);
      digitalWrite(12, LOW);
      digitalWrite(11, HIGH); //red on
      digitalWrite(10, HIGH); //put barriers down
      delay(500);
      digitalWrite(10, LOW);
      servoCCW(-90);
    }
  }

  //pass first sensor
  if(WayPoint == 1)
  {
    LDRValue0 = analogRead(LDR0);
    if(LDRValue0 < light_sensitivity0)
    {
      WayPoint += 1;
      digitalWrite(13, LOW);
      digitalWrite(12, HIGH); //yellow on
      digitalWrite(11, LOW);
      servoCCW(-90);
    }
  }

  //start
  if(WayPoint == 0)
  {
    servoCW(180);
    WayPoint += 1;
  }
}