turnTo

//Turn to a direction using RPS
void turnTo(float angle) {

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

 while ((RPS.Heading() + hCalibrate  > angle + 1 || RPS.Heading() + hCalibrate < angle - 1) && !stop && (start + TIMEOUT > TimeNow())) {
     showInfo();
     if (RPS.Heading() > angle) {



         if ((RPS.Heading() + hCalibrate - angle) > (angle + 360 - RPS.Heading() + hCalibrate)) {
             // TURN LEFT

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

             check = 1;

             rightMotor.SetPercent(OUTSIDE_TURN_POWER);
             leftMotor.SetPercent(INSIDE_TURN_POWER);
         } else {
             // TURN RIGHT

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

             check = -1;


             rightMotor.SetPercent(INSIDE_TURN_POWER);
             leftMotor.SetPercent(OUTSIDE_TURN_POWER);
         }

     } else {


         if ((angle - RPS.Heading() + hCalibrate) > (RPS.Heading() + hCalibrate + 360 - angle)) {
             // TURN RIGHT

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

             check = -1;

             rightMotor.SetPercent(INSIDE_TURN_POWER);
             leftMotor.SetPercent(OUTSIDE_TURN_POWER);
         } else {
             //TURN LEFT

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

             check = 1;

             rightMotor.SetPercent(OUTSIDE_TURN_POWER);
             leftMotor.SetPercent(INSIDE_TURN_POWER);
         }

     }

     Sleep(100);

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

     Sleep(100);

 }

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

Algorithm for turnTo:

1. Set check, stop, and start to initial values.
2. Run a loop that runs while the RPS heading + the heading calibration are greater than the angle + 1 or angle -1.
3. If rps heading is greater than the angle proceed to step 4, if not proceed to step 11.
4. If the rps heading + the calibrate – the angle is greater than the angle + 360 proceed to step 5, if not proceed to step 8.
5. If check is equal to -1, set stop equal to true.
6. Set check equal to 1.
7. Set the right motor to the outside power, and the left motor to the inside power. Proceed to step 15.
8. If the check is equal to 1, set stop equal to true.
9. Set check equal to -1.
10. Set the right motor to the inside power, and the left to the outside power. Proceed to step 15.
11. If the angle – rps heading + calibrate is greater than rps heading + calibrate + 360 – angle then proceed to step 12. If not proceed to step 15.
12. If the check is equal to 1, set stop equal to true.
13. Set check equal to -1.
14. Set the right motor to inside turn power, and the left to outside turn power. Go to step 15.
15. Turn off the right and left motors. If loop not broken go back to step 2.
16. Turn off the right and left motors.
17. End function.