I have continuously tested this PID, and it turns 90 degrees, but the goes back to 0 and does it again. Please tell me if you see something wrong with this code!
float turn_kP = 1; //The constant for error
float turn_kI = 1; //The constant for Integral
float turn_kD = 1; //The constant for derivative
float turn_error;
float turn_derivative;
float turn_integral;
float turn_Preverror;
float turn_pow;
void rtPID(int degreeamount)
{
while (true)//This makes the code run infinitely.
{
turn_error = degreeamount - Inertial17.heading(degrees);
turn_integral = turn_integral + turn_error;
wait(15,msec);
turn_Preverror = turn_error;
turn_derivative = turn_error - turn_Preverror;;
turn_pow = (turn_error*turn_kP)+(turn_integral*turn_kI)+(turn_derivative*turn_kD);
Left_Drive.spin(fwd, turn_pow, volt);
Right_Drive.spin(reverse, turn_pow, volt);
}
}
Can you show us the rest of the code where the function is being called? And you don’t want an infinite loop. Break out when the error is an acceptable value, like -0.1 to 0.1.
Don’t make it a while(true), make it a while(error > abs(1)). Otherwise, the loop will never exit even after hitting the setpoint, and will not go on to the next thing. This would also prevent your issue if you don’t have any other errors, I just skimmed through so I don’t know.
The main issue with this is the fact you dont have any exit condition and your pid is calculating the prevError at the wrong time. But I have a few suggestions:
You are calculating Integral, which isn’t necessary for a turn pid (in my experience) as it causes randomness and other issues
Your error calculation is a bit off, so lets say your desired was 359 and you are at 1 deg it will calculate that you have 359 degrees of error. There is a way to fix this you need to get the error inbetween -180 and 180 so that your pid goes the best direction for time and efficiency. This is what this would look like in code:
if(turn_error > 180){ // If the error is the non-optimal direction
turn_error -=360; // Subtract 360 to change the direction and get the distance away from 0 the other direction
}
else
{
if(turn_error < -180){ // Same thing but the error is negative this time
turn_error+=360; //Add 360 to change the direction and get the distance away from 0 the other direction
}
}
Your wait is in the wrong spot, currently you wait 15 msec between calculating the derivative and the error. This could cause potential issues with a error that was 15 msec ago. So instead your wait should either be at the beginning or at the end of the loop
You dont stop at the end of your loop, so if you dont have a stop outside of the function or a new function that controls the motors. It was just continue to spin.
So, If you apply these suggestions your turnPid should work more optimally/better.