checkMotors

//Checks to see if the motors are functional
void checkMotors() {

 //Motor check

 LCD.WriteRC("Initiating Motor Check ", 1, 0);
 Sleep(HALF_SECOND);
 LCD.WriteRC(".", 1, 22);
 Sleep(HALF_SECOND);
 LCD.WriteRC(".", 1, 23);
 Sleep(HALF_SECOND);
 LCD.WriteRC(".", 1, 24);
 Sleep(HALF_SECOND);

 LCD.Clear( FEHLCD::Black );

 LCD.WriteLine("Moving Wrench Servo!");
 wrenchServo.SetDegree(100);
 Sleep(HALF_SECOND);
 wrenchServo.SetDegree(50);
 Sleep(FULL_SECOND);

 LCD.WriteLine("Moving Fuel Servo!");
 fuelServo.SetDegree(100);
 Sleep(HALF_SECOND);
 fuelServo.SetDegree(50);
 Sleep(FULL_SECOND);

 LCD.WriteLine("Ok!  Testing Forward!");
 rightMotor.SetPercent(LOW_FORWARD);
 leftMotor.SetPercent(LOW_FORWARD);
 Sleep(FULL_SECOND);
 rightMotor.Stop();
 leftMotor.Stop();
 Sleep(HALF_SECOND);

 LCD.WriteLine("Now testing backwards!");
 rightMotor.SetPercent(-1 * LOW_FORWARD);
 leftMotor.SetPercent(-1 * LOW_FORWARD);
 Sleep(FULL_SECOND);
 rightMotor.Stop();
 leftMotor.Stop();
 Sleep(HALF_SECOND);

 LCD.WriteLine("Alright, going right now!");
 leftMotor.SetPercent(MID_FORWARD);
 Sleep(FULL_SECOND);
 leftMotor.Stop();
 Sleep(HALF_SECOND);

 LCD.WriteLine("And finally, left!");
 rightMotor.SetPercent(MID_FORWARD);
 Sleep(FULL_SECOND);
 rightMotor.Stop();
 Sleep(HALF_SECOND);
 rightMotor.Stop();

 LCD.Clear( FEHLCD::Black );

 return;

}

Algorithm for checkMotors:

1. Output that motor check is being initiated.
2. Output that wrench servo is being tested.
3. Move wrench servo up and down.
4. Output the fuel servod is being tested.
5. Move fuel servo left and right.
6. Output that the robot will test moving forward.
7. Drive forward slowly.
8. Output that the robot will now test driving backward.
9. Drive backward slowly.
10. Output that the robot will test turning right.
11. Turn right slowly.
12. Output that the robot will test turning left.
13. Turn left slowly.
14. End function.