pressButton

//Movement to press correct button
void pressButton(bool isBlue) {


 //After reading the color, make corresponding movements
 if(isBlue) {
     LCD.WriteLine("BLUE!");
     LCD.WriteLine(lightSensor.Value());

     //Orient
     moveForward(MID_FORWARD, 1.5);
     turnRight(90);
     turnTo(268);
     rightMotor.SetPercent(12);
     leftMotor.SetPercent(12);
     Sleep(0.3);
     moveForward(FAST, 17);
     moveForward(-1 * LOW_FORWARD, 3);
     rightMotor.SetPercent(100);
     Sleep(6.0);
     rightMotor.Stop();

     //Revert
     moveForward(-1 * MID_FORWARD, 7);
     turnRight(100);
     turnTo(180);
     moveForward(MID_FORWARD, 4.5);


 } else {
     LCD.WriteLine("RED!");
     LCD.WriteLine(lightSensor.Value());

     //Orient
     moveForward(-1 * MID_FORWARD, 6);
     turnRight(90);
     turnTo(270);
     //moveForward(MID_FORWARD,15);
     moveForward(FAST, 17);
     moveForward(-1 * LOW_FORWARD, 3);
     leftMotor.SetPercent(100);
     Sleep(6.0);
     leftMotor.Stop();

     //Revert
     moveForward(-1 * MID_FORWARD, 7);
     turnRight(60);
     turnTo(180);

 }


}

Algorithm for pressButton:

1. Check if the light is blue, if it is proceed to step 2, if it is not proceed to step 5.
2. Orient robot so that it is in front of the blue light.
3. Drive forward and hit the blue light with the chassis.
4. Revert back to previous position. Proceed to step 8.
5. Orient robot so that it is in front of the red light.
6. Drive forward and hit the red light with the chassis.
7. Revert back to previous position.
8. End function.