moveToY

//Move to an y coord using RPS
void moveToY(float y) {

 int direction = 1;
 int check = 0;
 bool stop = false;
 float start = TimeNow();

 if(RPS.Heading() < 360 && RPS.Heading() > 180) {
     //Facing the negative y direction
    direction = -1;

 }

 while( (RPS.Y() + yCalibrate < y - 0.3 || RPS.Y() + yCalibrate > y + 0.3)  && !stop && (start + TIMEOUT > TimeNow()))  {

     if( RPS.Y() < y) {

         if (check == -1) {
             stop = true; //count++
         }

         check = 1;

         rightMotor.SetPercent(direction * 12);
         leftMotor.SetPercent(direction * 12);

     } else {

         if (check == 1) {
             stop = true; //count++
         }

         check = -1;

         rightMotor.SetPercent(direction * -12);
         leftMotor.SetPercent(direction * -12);

     }

     Sleep(50);

     rightMotor.Stop();
     leftMotor.Stop();

     Sleep(100);

 }

}

Algorithm for moveToY:

1. Set the direction to negative, check to 0, stop to false, and start to the current time.
2. If the RPS heading is between 360 and 180 degrees set direction to negative.
3. Begin a loop that runs while the Y location + the yCalibrate are less than y – 0.3, or greater than y + 0.3.
4. If the rps of y is less than variable y proceed to step 5, if not proceed to step 8.
5. If check is -1 set stop equal to true.
6. Set check = 1.
7. Set the right and left motor percents to direction * 12.
8. If the check is equal to 1, set stop equal to true.
9. Set check equal to -1.
10. Set the right and left motor percents to direction * -12.
11. Turn off motors.
12. Exit loop when conditions met.
13. End function.