Final Competition

     setFuel();

     moveForward(HIGH_FORWARD, 11);
     moveToY(22.5);

     turnRight(-77);
     turnTo(358);

     moveForward(HIGH_FORWARD, 17);
     moveToX(25.5);

     pressButton(isBlue());

     moveForward(HIGH_FORWARD, 13)

     turnRight(-35);

     //THE WRENCH

     wrenchServo.SetDegree(140);         // Set the servos

     moveForward(50, 12);
     moveToY(18.55);
     showInfo();
    
     // Line up with wrench

     turnRight(30);
     turnTo(180);
     showInfo();

     // Turn towards wrench

     moveForward(LOW_FORWARD, 17);
     showInfo();
     
     // Approach wrench

     while(RPS.X() > 8.5) {
         moveForward(-1 * LOW_FORWARD, 3);

         if(RPS.Y() > 16.8) {
             turnRight(-5);
         } else {
             turnRight(5);
         }

         moveForward(LOW_FORWARD, 10);

     }

     wrenchServo.SetDegree(40);
     Sleep(HALF_SECOND);

     turnTo(180);

     moveForward(-50, 6);
     showInfo();
                      // Back off from wrench

     turnRight(-66);
     showInfo();
                     // Turn to approach jack

     moveForward(50, 12);
     moveToY(11);
     showInfo();
                      // Approach jack area

     turnRight(-92);
     showInfo();
                      // Turn to jack

     moveForward(50, 12);
     showInfo();
                      // Push the jack

     moveForward(-50, 5);
     showInfo();
                     // Back off

     turnRight(85);
     showInfo();
                   // Turn to ramp

     moveForward(-50, 15.5);
     moveToY(21.5);
     showInfo();
                      // Move to ramp

     turnRight(-42);
     turnTo(315);
     showInfo();
                     // Adjust on ramp

     moveForward(-50, 10);
     moveToX(4);
     showInfo();
                      // Move to ramp

     turnRight(35);
     turnTo(274);
     showInfo();
                     // Adjust on ramp

     bool deadzone;
     deadzone = (RPS.IsDeadzoneActive() != 2);


     // UP TOP

     rightMotor.SetPercent(-12);
     leftMotor.SetPercent(-12);
     Sleep(0.3);
     rightMotor.SetPercent(-100);
     leftMotor.SetPercent(-100);
     Sleep(1.2);
     rightMotor.SetPercent(-50);
     leftMotor.SetPercent(-50);
     Sleep(1.05);
     rightMotor.Stop();
     leftMotor.Stop();


     Sleep(300);

     moveForward(MID_FORWARD, 30);
     moveForward(-1 * MID_FORWARD, 10);
     turnRight(-90);
     moveForward(-1 * MID_FORWARD, 8);


     if(!deadzone) {
         moveToY(46);
     }

     turnRight(38);
     if(!deadzone && RPS.X() != -1) {
         turnTo(226);
     }

     moveForward(-1 * HIGH_FORWARD, 20);
     if(!deadzone && RPS.X() != -1) {
         turnTo(226);
     }

     moveForward(-1 * HIGH_FORWARD, 20);

     moveForward(-1 * HIGH_FORWARD, 10);


     turnFuel();                         // FUEL STUFF


     moveForward(HIGH_FORWARD, 16);
     if(!deadzone && RPS.X() != -1) {
         moveToX(19);
     }

     turnRight(75);
     if(!deadzone && RPS.X() != -1) {
         turnTo(134);
     }
     moveForward(HIGH_FORWARD, 30);
     moveForward(-1 * MID_FORWARD, 2);
     wrenchServo.SetDegree(150);         // Place wrench in garage
     Sleep(FULL_SECOND);
     Sleep(0.5);
     moveForward(-1 * FAST, 45);         // Go to the ramp again
     wrenchServo.SetDegree(40);
     turnRight(47);                      // Turn to ramp
     moveForward(-1 * FAST, 17);         // Down we go
     turnRight(105);
     moveForward(-1 * HIGH_FORWARD, 20);
     turnRight(105);
     moveForward(-1 * HIGH_FORWARD, 30);

     break;

Algorithm for Main Competition:

1. Check fuel direction, and turn the fuel servo accordingly
2. Move forward, then correct the Y position
3. Turn left, then correct heading
4. Move forward, then correct the X position
5. Read the color of the light and press the corresponding button
6. Move forward
7. Turn right
8. Lower wrench servo
9. Move forward, then correct Y position
10. Turn right, then correct heading
11. Move forward at a low speed
12. If at the incorrect X position, turn 5 degrees in the correct direction and go back to step 13
13. Raise wrench servo
14. Move backwards
15. Turn left
16. Move forward, then correct Y position
17. Turn left
18. Move forward, pushing jack
19. Move backwards
20. Turn right
21. Move backwards, then correct Y position
22. Turn left, then correct heading
23. Move backwards, then correct X position
24. Turn right, then correct heading
25. Store whether or not the deadzone is active
26. If the deadzone is active, ignore all RPS checks on the upper level
27. Move backwards at slowest speed
28. Move backwards at full power
29. Move backwards at half power
30. Move forward to square with wall
31. Move backwards
32. Turn left
33. Move backwards
34. Turn right, then correct heading
35. Move backwards, correcting heading during movement
36. Turn the fuel crank in the correct direction
37. Move forward, then correct X position
38. Turn right, then correct heading
39. Move forward
40. Lower wrench servo
41. Move backwards
42. Raise wrench servo
43. Turn right
44. Move backwards down the ramp
45. Turn right
46. Move backwards
47. Turn right
48. Move backwards into final button