Our main coder has developed a PID loop based function to make our robot turn more precise. The robot turns at the degree angle, but sometimes it doesn’t turn in the direction we want it to go.

Here’s our turn function:

```
void turn(double heading, int direction, double multiplier) //Heading will always be between 0 and 359.99 degrees (if turning counterclockwise, heading is measured counterclockwise and vice versa), direction 0 is clockwise and direction 1 is counterclockwise
{
double kp, kd;
double value = 1;
double lastError, error, derivative, speed;
kp = value; //determined through testing, reduces time taken but also increases overshoot
kd = value; //determined through testing, increases smoothness and decreases overshoot, yet shouldn’t get too large
if(direction == 1) //If turning clockwise/right
{
//iSensor.setHeading(0.01, vex::degrees);
//lastError = heading - iSensor.heading(); //The initial error for the derivative
lastError = heading - iSensor.heading(); //The initial error for the derivative
while(std::abs(heading - iSensor.heading()) > 5) //While the desired and inertial sensor headings are different
{
error = heading - iSensor.heading(); //The number of degrees needed until the desired heading is reached
derivative = error - lastError; //The change in error
lastError = error; //The last error used in the derivative
speed = (kp*error + kd*derivative); //Motor speeds
map(speed, 0, kp*360, 0, multiplier*100); //Scale speed from 0-(max speed) to 0-100 for percent motor speed (maximum error is 360 degrees)
frontRight.spin(vex::directionType::fwd, speed, vex::velocityUnits::pct); //Front right motor is reversed
frontLeft.spin(vex::directionType::fwd, speed, vex::velocityUnits::pct);
}
}
else if(direction == 0) //If turning counterclockwise/left
{
//lastError = heading - map(iSensor.heading(), 0, 359.99, 359.99, 0); //Initial remapped error for the derivative
lastError = heading - map(iSensor.heading(), 0, 359.99, 359.99, 0); //Initial remapped error for the derivative
while(std::abs(heading - map(iSensor.heading(), 0, 359.99, 359.99, 0)) > 5) //While the desired and inertial sensor headings are different (heading orientation measurement reversed)
{
error = heading - map(iSensor.heading(), 0, 359.99, 359.99, 0); //The number of degrees needed until the desired (remapped) heading is reached
derivative = error - lastError; //The change in error
lastError = error; //The last error used in the derivative
speed = (kp*error + kd*derivative); //Motor speeds
map(speed, 0, kp*360, 0, multiplier*100); //Scale speed from 0-(max speed) to 0-100 for percent motor speed (maximum error is 360 degrees)
frontRight.spin(vex::directionType::rev, speed, vex::velocityUnits::pct); //Front right motor is reversed
frontLeft.spin(vex::directionType::rev, speed, vex::velocityUnits::pct);
}
}
frontLeft.stop(vex::brakeType::hold);
frontRight.stop(vex::brakeType::hold);
}
```

Some observations we have made:

*Near the 0/360 degree mark, if it’s pointing near the 0 to 90 degree area, it turns clockwise. If it’s pointing near the 270 to 360 degree area, it turns counter-clockwise.

The main thing we want to fix is to ensure that it turns only clockwise or counter-clockwise when told to. Any suggestions would be great.

And thank you to all who have help suggest some solutions before.