Drive

This code contains all of the robot drive functions.

#ifndef DRIVE_H
#define DRIVE_H
#include "constants.h"

void driveUntilTime(int heading, float power, int time, bool stop);
void driveWhileRotate(int startHeading, float turnDegree, float power, float turnPower, int time);
void driveUntilBump(int heading, float power, int bumpSide);
int driveUntilBumpTimeout(int heading, float power, int bumpSide, int timeout);
void driveUntilCds(int heading, float power, float cdsvalue, int timeout);
void driveUntilRPSx(float desiredX, float power, int faultHeading, int timeout);
void driveUntilRPSy(float desiredY, float power, int faultHeading, int timeout);
void driveUntilRPSxRange(float desiredX, float power, int faultHeading, float range, int timeout);
void driveUntilRPSyRange(float desiredY, float power, int faultHeading, float range, int timeout);

void turnUntilTime(float power, int time);
void turnUntilRPS(int desiredHeading, int power, int timeout);

#endif // DRIVE_H

// Required FEH libraries
#include <FEHLCD.h>
#include <FEHBuzzer.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHIO.h>
#include <FEHRPS.h>
#include <FEHSD.h>

// Required library to calculate motor ratios based on heading
#include <math.h>

// Required custom libraries
#include "constants.h"
#include "worldstate.h"
#include "drive.h"

//Declare global motor ratios
float motor1ratio;
float motor2ratio;
float motor3ratio;
float motor4ratio;

/* This function calculates the motor ratios for a given heading. The calculation is derived from vector quantities.
 * The calculated ratios are assigned to each individual motor ratio. The ratios range from -1 to 1.
 * If the heading is a cardinal direction (N/S/E/W), the ratio is 0.707.
 * Last Modified: 3/14/2016 JKL
 */
void ratios(int heading) {

    LCD.WriteRC("Calculating motor ratios", 5,3);
    float x, y;
    x = cos( (45-heading) * PI / 180.0 );
    y = sin( (45-heading) * PI / 180.0 );
    LCD.WriteRC(x, 6, 3);
    LCD.WriteRC(y, 7, 3);
    motor1ratio = x;
    motor2ratio = -y;
    motor3ratio = -x;
    motor4ratio = y;
} // end ratios function

/* This function drives the robot with a given heading at a given power for a given time in milleseconds.
 * If bool stop is false, the robot does not stop the motors. There will not be a pause in between the next drive function.
 * Last Modified: 3/15/2016 JKL
 */
void driveUntilTime(int heading, float power, int time, bool stop) {

    ratios(heading);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilTime start");
    LCD.WriteRC("Time:", 5, 3);
    LCD.WriteRC(time, 5, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    Sleep(time);
    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilTime stop");
    if (stop) {
        motor1.SetPercent(0);
        motor2.SetPercent(0);
        motor3.SetPercent(0);
        motor4.SetPercent(0);

        Sleep(100);
    }
} // end driveUntilTime function

/* This function drives the robot while rotating it.
 * startHeading: the initial heading the robot drives.
 * turnDegree: the total degrees the robot rotates during the drive
 * power: the power the robot drives at; changing this does not affect the total turn degree or drift angle
 * turnPower: the constant power added to turn the robot; this is dependent upon turnDegree and time
 * time: the duration the robot drives in milleseconds
 * Last modified: 4/8/2016 JKL
 */
void driveWhileRotate(int startHeading, float turnDegree, float power, float turnPower, int time) {
    // x is a fraction of the turnDegree used to add to the increment
    float x = turnDegree / (time / 50.0);
    for( int increment = 0; increment <= turnDegree; increment += x) {
        ratios(startHeading + increment);
        motor1.SetPercent(power * motor1ratio + turnPower);
        motor2.SetPercent(power * motor2ratio + turnPower);
        motor3.SetPercent(power * motor3ratio + turnPower);
        motor4.SetPercent(power * motor4ratio + turnPower);
        Sleep(50);
    }
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
}

/* This function drives the robot with a given heading at a given power until a side is bumped (or not).
 * int bumpSide ranges from -4 to 4. The sides are numbered in a clockwise fashion. The front of the robot is side 1.
 * A positive value allows for a robot drive until that side is bumped.
 * A negative value allows for the robot to ride along that side until a microswitch is released.
 * Last Modified: 3/14/2016
 */
void driveUntilBump(int heading, float power, int bumpSide) {

    ratios(heading);

    LCD.WriteRC("Bump:", 5, 3);
    LCD.WriteRC(bumpSide, 5, 9);
    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilBump start");
    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    // if any of the microswitches are released on the specified side
    if (bumpSide == -1)
        while( 2 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == -2)
        while( 2 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == -3)
        while( 2 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == -4)
        while( 2 != (microswitch7.Value() + microswitch8.Value()) );

    // if both of the microswitches are depressed on the specified side
    if (bumpSide == 1)
        while( 0 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == 2)
        while( 0 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == 3)
        while( 0 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == 4)
        while( 0 != (microswitch7.Value() + microswitch8.Value()) );

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilBump stop");
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(100);
} // end driveUntilBump function

/* This function drives the robot with a given heading at a given power until a side is bumped (or not).
 * int bumpSide ranges from -4 to 4. The sides are numbered in a clockwise fashion. The front of the robot is side 1.
 * A positive value allows for a robot drive until that side is bumped.
 * A negative value allows for the robot to ride along that side until a microswitch is released.
 * int timeout limits the duration the robot drives in milleseconds
 * The return value is 0 is the function fulfills bumpSide or 1 if the function times out.
 * Last Modified: 3/31/2016
 */
int driveUntilBumpTimeout(int heading, float power, int bumpSide, int timeout) {

    ratios(heading);

    LCD.WriteRC("Bump:", 5, 3);
    LCD.WriteRC(bumpSide, 5, 9);
    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilBumpTimeout start");
    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    int returnValue = 0;
    int startTime = TimeNowMSec();
    // if any of the microswitches are released on the specified side
    if (bumpSide == -1)
        while( 2 != (microswitch1.Value() + microswitch2.Value()) && TimeNowMSec() - startTime < timeout ) {};
    if (bumpSide == -2)
        while( 2 != (microswitch3.Value() + microswitch4.Value()) && TimeNowMSec() - startTime < timeout ) {};
    if (bumpSide == -3)
        while( 2 != (microswitch5.Value() + microswitch6.Value()) && TimeNowMSec() - startTime < timeout ) {};
    if (bumpSide == -4)
        while( 2 != (microswitch7.Value() + microswitch8.Value()) && TimeNowMSec() - startTime < timeout ) {};

    // if both of the microswitches are depressed on the specified side
    if (bumpSide == 1)
        while( 0 != (microswitch1.Value() + microswitch2.Value()) && TimeNowMSec() - startTime < timeout ) {};
    if (bumpSide == 2)
        while( 0 != (microswitch3.Value() + microswitch4.Value()) && TimeNowMSec() - startTime < timeout ) {};
    if (bumpSide == 3)
        while( 0 != (microswitch5.Value() + microswitch6.Value()) && TimeNowMSec() - startTime < timeout ) {};
    if (bumpSide == 4)
        while( 0 != (microswitch7.Value() + microswitch8.Value()) && TimeNowMSec() - startTime < timeout ) {};

    if (TimeNowMSec() - startTime > timeout)
        returnValue = 1;

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilBumpTimeout stop");
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(100);
    return returnValue;
} // end driveUntilBumpTimeout function

/* This function drives the robot with a given heading at a given power until the cdsvalue is reached.
 * If the CdS sensor reads a value less than float cdsvalue, the function stops the robot.
 * Last Modified: 3/25/2016 JKL
 */
void driveUntilCds(int heading, float power, float cdsvalue, int timeout) {

    ratios(heading);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilCds start");
    LCD.WriteRC("CdS:", 5, 3);
    LCD.WriteRC(cdsvalue, 5, 8);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    int startTime = TimeNowMSec();
    while( cdscell.Value() > cdsvalue && TimeNowMSec() - startTime < timeout);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    SD.Printf("driveUntilCds stop");

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(150);
} // end driveUntilCds function

/* This function corrects the x-position of the robot using RPS.
 * The robot will drive until the desiredX coordinate with a given power or until the timeout is exceeded.
 * If the RPS is -1 or -2 (off the course or dead zone), the robot will pulse in the direction of faultHeading.
 * Use driveUntilRPSxRange if a custom range/tolerance is needed.
 * Last Modified: 3/28/2016 JKL
 */
void driveUntilRPSx(float desiredX, float power, int faultHeading, int timeout) {

    float startTime = TimeNowMSec();
    // the tolerance for the X coordinate is +/- 0.25 inches
    while((RPS.X() < desiredX - 0.25 || RPS.X() > desiredX + 0.25) && TimeNowMSec() - startTime < timeout) {

        // pulse toward faultHeading
        if (RPS.X() == -1 || RPS.X() == -2) {
            driveUntilTime(faultHeading, power, 500, true);
        }
        else if (RPS.X() < desiredX) {
            int heading = RPS.Heading() + 90;
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSx");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // continuously drive the motors while the robot is greater than 2 inches away the desiredX
            while( fabs( desiredX - RPS.X() ) >= 2);
            // pulse the motors if the robot is within 2 inches of the desiredX
            if ( fabs( desiredX - RPS.X() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        else if (RPS.X() > desiredX) {
            int heading = RPS.Heading() - 90;
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSx");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // continuously drive the motors while the robot is greater than 2 inches away the desiredX
            while( fabs( desiredX - RPS.X() ) >= 2);
            // pulse the motors if the robot is within 2 inches of the desiredX
            if ( fabs( desiredX - RPS.X() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    } // end while loop
} // end driveUntilRPSx function

/* This function corrects the y-position of the robot using RPS.
 * The robot will drive until the desiredY coordinate with a given power or until the timeout is exceeded.
 * If the RPS is -1 or -2 (off the course or dead zone), the robot will pulse in the direction of faultHeading.
 * Use driveUntilRPSyRange if a custom range/tolerance is needed.
 * Last Modified: 3/28/2016 JKL
 */
void driveUntilRPSy(float desiredY, float power, int faultHeading, int timeout) {
    float startTime = TimeNowMSec();
    // the tolerance for the Y coordinate is +/- 0.25 inches
    while((RPS.Y() < desiredY - 0.25 || RPS.Y() > desiredY + 0.25) && TimeNowMSec() - startTime < timeout) {

        // pulse toward faultHeading
        if (RPS.Y() == -1 || RPS.Y() == -2) {
            driveUntilTime(faultHeading, power, 200, true);
        }
        else if (RPS.Y() < desiredY) {
            int heading = RPS.Heading();
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSy");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredY, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // continuously drive the motors while the robot is greater than 2 inches away the desiredY
            while( fabs( desiredY - RPS.Y() ) >= 2);
            // pulse the motors if the robot is within 2 inches of the desiredY
            if ( fabs( desiredY - RPS.Y() ) < 2)
                Sleep(250);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        else if (RPS.Y() > desiredY) {
            int heading = RPS.Heading() + 180;
            heading-= 360 * (heading < 360);
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSy");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredY, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // continuously drive the motors while the robot is greater than 2 inches away the desiredY
            while( fabs( desiredY - RPS.Y() ) >= 2);
            // pulse the motors if the robot is within 2 inches of the desiredY
            if ( fabs( desiredY - RPS.Y() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    } // end while loop
} // end driveUntilRPSy function

/* This function corrects the x-position of the robot using RPS.
 * The robot will drive until the desiredX coordinate with a given power or until the timeout is exceeded.
 * If the RPS is -1 or -2 (off the course or dead zone), the robot will pulse in the direction of faultHeading.
 * float range is the distance error from the desiredX the robot can be within
 * Last Modified: 4/2/2016 JKL
 */
void driveUntilRPSxRange(float desiredX, float power, int faultHeading, float range, int timeout) {
    float startTime = TimeNowMSec();
    while((RPS.X() < desiredX - range || RPS.X() > desiredX + range) && TimeNowMSec() - startTime < timeout) {
        if (RPS.X() == -1 || RPS.X() == -2) {
            driveUntilTime(faultHeading, power, 500, true);
        }
        else if (RPS.X() < desiredX) {
            int heading = RPS.Heading() + 90;
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSx");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // to pulse or not to pulse
            while( fabs( desiredX - RPS.X() ) >= 2);
            if ( fabs( desiredX - RPS.X() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        else if (RPS.X() > desiredX) {
            int heading = RPS.Heading() - 90;
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSx");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // to pulse or not to pulse
            while( fabs( desiredX - RPS.X() ) >= 2);
            if ( fabs( desiredX - RPS.X() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    } // end while loop
} // end driveUntilRPSxRange function

/* This function corrects the y-position of the robot using RPS.
 * The robot will drive until the desiredY coordinate with a given power or until the timeout is exceeded.
 * If the RPS is -1 or -2 (off the course or dead zone), the robot will pulse in the direction of faultHeading.
 * float range is the distance error from the desiredY the robot can be within
 * Last Modified: 4/2/2016 JKL
 */
void driveUntilRPSyRange(float desiredY, float power, int faultHeading, float range, int timeout) {
    float startTime = TimeNowMSec();
    while((RPS.Y() < desiredY - range || RPS.Y() > desiredY + range) && TimeNowMSec() - startTime < timeout) {
        if (RPS.Y() == -1 || RPS.Y() == -2) {
            driveUntilTime(faultHeading, power, 500, true);
        }
        else if (RPS.Y() < desiredY) {
            int heading = RPS.Heading();
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSy");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredY, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            // continuously drive the motors while the robot is greater than 2 inches away the desiredY
            while( fabs( desiredY - RPS.Y() ) >= 2);
            // pulse the motors if the robot is within 2 inches of the desiredY
            if ( fabs( desiredY - RPS.Y() ) < 2)
                Sleep(250);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        else if (RPS.Y() > desiredY) {
            int heading = RPS.Heading() + 180;
            heading-= 360 * (heading < 360);
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            SD.Printf("driveUntilRPSy");
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredY, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            while( fabs( desiredY - RPS.Y() ) >= 2);
            // to pulse or not to pulse
            if ( fabs( desiredY - RPS.Y() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    } // end while loop
} // end driveUntilRPSyRange function

/* This function turns the robot at a given power for a given time.
 * float power (0-100): positive = clockwise rotation, negative = counter-clockwise rotation
 * Last Modified: 4/2/2016 JKL
 */
void turnUntilTime(float power, int time) {

    worldState(true, 0, power, 1, 1, 1, 1);
    SD.Printf("turnUntilTime start");
    LCD.WriteRC("Time:", 5, 3);
    LCD.WriteRC(time, 5, 9);

    motor1.SetPercent(power);
    motor2.SetPercent(power);
    motor3.SetPercent(power);
    motor4.SetPercent(power);

    Sleep(time);

    worldState(true, 0, power, 1, 1, 1, 1);
    SD.Printf("turnUntilTime stop");

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(100);

} // end turnUntilTime function

/* This function corrects the heading of the robot using RPS.
 * Power should be less than 30.
 * Last Modified: 4/2/2016 JKL
 */
void turnUntilRPS(int desiredHeading, int power, int timeout) {
    int startTime = TimeNowMSec();
    // tolarance/range is +/- 1.5 degrees
    // probably could have implemented a custom range
    while( ( fabs(desiredHeading-RPS.Heading())>1.5 && fabs(desiredHeading-RPS.Heading())<358.5 ) && (TimeNowMSec() - startTime < timeout)) {
        worldState(true, desiredHeading, power, 1,1,1,1);
        SD.Printf("turnUntilRPS");
        LCD.WriteRC("Desired H:", 5, 3);
        LCD.WriteRC(desiredHeading, 5, 13);

        // if within +/- 10 degree of desiredHeading, start to pulse the motors
        if ( fabs(desiredHeading-RPS.Heading())<10 || fabs(desiredHeading-RPS.Heading())>350) {
            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);
            worldState(true, desiredHeading, power, 0,0,0,0);
            SD.Printf("turnUntilRPS");
            Sleep(150);
        }

        // if within +/- 1.5 degrees, stop everything and break out of the function
        // necessary to check within the loop right after the robot has stopped from the pulsing
        if (fabs(desiredHeading-RPS.Heading())<1.5 || fabs(desiredHeading-RPS.Heading())>358.5) {
            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);
            worldState(true, desiredHeading, power, 0,0,0,0);
            SD.Printf("turnUntilRPS");
            return;
        }

        if (RPS.Heading() - desiredHeading >= 0 ) {

            // turn clockwise
            if (RPS.Heading() - desiredHeading <= 180) {
                motor1.SetPercent(power);
                motor2.SetPercent(power);
                motor3.SetPercent(power);
                motor4.SetPercent(power);
            }

            // turn counter-clockwise
            else if (desiredHeading - RPS.Heading() < 180) {
                motor1.SetPercent(-power);
                motor2.SetPercent(-power);
                motor3.SetPercent(-power);
                motor4.SetPercent(-power);

            }
        }

        if (RPS.Heading() - desiredHeading < 0 ) {

            // turn counter-clockwise
            if (desiredHeading - RPS.Heading() <= 180) {
                motor1.SetPercent(-power);
                motor2.SetPercent(-power);
                motor3.SetPercent(-power);
                motor4.SetPercent(-power);
            }

            // turn clockwise
            else if (RPS.Heading() - desiredHeading < 180) {
                motor1.SetPercent(power);
                motor2.SetPercent(power);
                motor3.SetPercent(power);
                motor4.SetPercent(power);
            }
        }
    } // end while loop
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
    worldState(true, desiredHeading, power, 0,0,0,0);
    SD.Printf("turnUntilRPS");
} // end turnUntilRPS function