main

//Figure out connections
FEHMotor rightMotor(FEHMotor::Motor0,POWER);
FEHMotor leftMotor(FEHMotor::Motor1,POWER);
FEHServo wrenchServo(FEHServo::Servo7);
FEHServo fuelServo(FEHServo::Servo0);
DigitalEncoder rightEncoder(FEHIO::P1_1);
DigitalEncoder leftEncoder(FEHIO::P1_0);
AnalogInputPin lightSensor(FEHIO::P0_1);


int startUp();
void checkMotors();
void showInfo();
void turnRight(float angle);
void moveForward(int percent, float distance);
void readyStart();
float abs(float value);
void runCourse(int test);
bool isBlue();
void pressButton(bool isBlue);
bool ask(char question[]);
void setFuel();
void turnFuel();
void turnTo(float angle);
void moveToX(float x);
void moveToY(float y);

void mainToWrench();
void wrenchToJack();

bool isClockwise;

float xCalibrate;
float yCalibrate;
float hCalibrate;


bool testing = true;


//270 points towards -x
//360 points towards +y
//x is jack direction

int main(void)
{

 int course = startUp();

 if(course != -1) {
     LCD.WriteLine("Ok, I'll run that course!");
     Sleep(FULL_SECOND);
     LCD.WriteLine("Wish me luck!");
     Sleep(FULL_SECOND);

     runCourse(course);

     if(ask("Did I do it right???")) {
         LCD.WriteLine("Yay I did it!");
         Sleep(HALF_SECOND);
         LCD.WriteLine("Was that an official test?");
         Sleep(HALF_SECOND);
         if(ask("Was it?!")) {
             LCD.WriteLine("Yes!  Full points!");
         } else {
             LCD.WriteLine("Lets make it official then");
             Sleep(HALF_SECOND);
             LCD.WriteLine("I got this!");
             Sleep(HALF_SECOND);
             LCD.WriteLine(" ");
             LCD.WriteLine(":)");
         }
     } else {
         LCD.WriteRC("Aw",0,0);
         Sleep(HALF_SECOND);
         LCD.WriteRC(".", 0, 2);
         Sleep(HALF_SECOND);
         LCD.WriteRC(".", 0, 3);
         Sleep(HALF_SECOND);
         LCD.WriteRC(".", 0, 4);
         Sleep(HALF_SECOND);
         LCD.WriteRC("I'll do better next time,",1,0);
         LCD.WriteRC("promise!",2,0);
     }

 } else {
     LCD.WriteLine("Guess I'm done then!");
     LCD.WriteLine("Good night!");
 }


 return 0;
}

Algorithm for main:

1. Set variable course to equal the startUp function.
2. Output which test is being run.
3. Run the course using runCourse.
4. Ask user if the test was successful, if yes go to step 5, if no go to step 6.
5. Output “Yay I did it!” then “Was that an official test?” If yes, output “Yes! Full points!”, if no, output “Let’s make it official then. I got this! :)”
6. Output “Aw…I’ll do better next time, promise!”
7. Output “Guess I’m done then! Good night!” once the user is done.