I have been trying to use @Connor 's PID example code to create my own PID loop. I was able to use it to create a straight line code that works. However, the same template does not work for a turn. The code is below.

```
// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
using namespace vex;
motor(LeftMotorFront) = motor(PORT10, true);
motor(LeftMotorBack) = motor(PORT3);
motor(RightMotorBack) = motor(PORT13, true);
motor(RightMotorFront) = motor(PORT20);
double KdP = 0.01;
double C = 2;
double KdD = 0.007;
double KsP = 0.05;
double KsI = 0.01;
double KsD = 0.01;
double KtP = 1;
double KtI = 1;
double KtD = 0;
float degToMdeg = atan(0.6) * sqrt(76.5);
int robotwidth = 9;
float error = 0;
float preverror = 0;
float deriv = 0;
float integral = 0;
float turnerror = 0;
float turnintegral = 0;
float turnderiv = 0;
float turnpreverror = 0;
float dist = 200;
float theta = 45;
int radius = 0;
int pointturn = 0;
bool resetencoders = false;
bool enablePIDstraight = false;
bool enablePIDpturn = true;
int PIDstraight(){
while(enablePIDstraight){
if(resetencoders){
resetencoders = false;
LeftMotorFront.setPosition(0, rotationUnits::deg);
LeftMotorBack.setPosition(0, rotationUnits::deg);
RightMotorFront.setPosition(0, rotationUnits::deg);
RightMotorBack.setPosition(0, rotationUnits::deg);
}
int LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
Brain.Screen.newLine();
Brain.Screen.print(turnerror);
int RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
//Get average of the two motors
int averagePosition = (LeftMotorPos + RightMotorPos)/2;
//Potential
error = dist - averagePosition ;
//Derivative
deriv = error - preverror;
//Integral
integral += error;
double lateralMotorPower = error * KdP + deriv * KdD;
if (error < 0.8 * dist){lateralMotorPower += C;}
/////////////////////////////////////////////////////////////////////
///////////////////////////////////////////
//Turning movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference = LeftMotorPos - RightMotorPos;
//Potential
turnerror = turnDifference;
//Derivative
turnderiv = turnerror - turnpreverror;
//Integral
turnintegral += turnerror;
if(error < 0.5){
turnintegral = 0;
}
double turnMotorPower = turnerror * KsP + turnderiv * KsD + turnintegral * KsI;
/////////////////////////////////////////////////////////////////////
LeftMotorFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
RightMotorBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotorFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
LeftMotorBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
preverror = error;
turnpreverror = turnerror;
vex::task::sleep(15);
}
return 1;
}
int PIDgradturn(){
if (resetencoders){
resetencoders = false;
LeftMotorFront.setPosition(0, rotationUnits::deg);
LeftMotorBack.setPosition(0, rotationUnits::deg);
RightMotorFront.setPosition(0, rotationUnits::deg);
RightMotorBack.setPosition(0, rotationUnits::deg);
}
int LeftMotorPos = (LeftMotorFront.rotation(deg)+LeftMotorBack.rotation(deg))/2;
Brain.Screen.newLine();
Brain.Screen.print(turnerror);
int RightMotorPos = (RightMotorFront.rotation(deg)+RightMotorBack.rotation(deg))/2;
float Thetaratio = (robotwidth+radius)/radius;
error = dist - (LeftMotorPos + RightMotorPos)/2;
//BigMotorIdeal = SmallMotorPos * Thetaratio;
return 1;
}
int PIDpointturn(){
while(enablePIDpturn){
if (resetencoders){
resetencoders = false;
LeftMotorFront.setPosition(0, rotationUnits::deg);
LeftMotorBack.setPosition(0, rotationUnits::deg);
RightMotorFront.setPosition(0, rotationUnits::deg);
RightMotorBack.setPosition(0, rotationUnits::deg);
}
float LeftMotorPos = (LeftMotorFront.rotation(rotationUnits::deg)+std::abs(LeftMotorBack.rotation(rotationUnits::deg)))/2;
float RightMotorPos = (std::abs(RightMotorFront.rotation(deg))+RightMotorBack.rotation(deg))/2;
Brain.Screen.newLine();
Brain.Screen.print(RightMotorPos);
error = dist - (std::abs(LeftMotorPos) + std::abs(RightMotorPos))/2;
integral += error;
deriv = error - preverror;
float lateralMotorPower = error * KtP + integral * KtI + deriv * KtD + C ;
LeftMotorFront.spin(forward, -lateralMotorPower , voltageUnits::volt);
RightMotorBack.spin(forward, -lateralMotorPower, voltageUnits::volt);
RightMotorFront.spin(forward, lateralMotorPower, voltageUnits::volt);
LeftMotorBack.spin(forward, lateralMotorPower , voltageUnits::volt);
task::sleep(15);
preverror = error;
}
return 1;
}
void Autonomous(){
task DriveStraightTask (PIDstraight);
//task GradTurn(PIDgradturn);
task PointTurn(PIDpointturn);
dist = 45 * degToMdeg;
task::sleep(1000);
waitUntil( error < 0.5);
enablePIDpturn = false;
}
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
Autonomous();
}
```

Note: The only reason I posted my whole code is so that is there is an error outside of my function, we can find it.

So basically, when I try to execute PIDpointturn (which is supposed to be a 45 degree turn), the robot turns forever. I have tried for the past 4 hours to get it to stop at the right value, but with no success.

Edit: I really need to fix this problem as my team needs a reliable autonomous in 3 days for State.