I tried a bunch of suggestions from other people, however my PID still does not work. When I run the code, it simply spins at max speed without stopping, and turning kP lower only deacreases the max speed. After making the brain print the error, derivative, and power, it showed that they were working fine. I have no idea what is wrong with my code, please help!
bool done = true;
float count = 0;
float turn_kP =0.51; //The constant for error
float turn_kD = 0.2; //The constant for derivative
float turn_error;
float turn_derivative;
float turn_Preverror;
float turn_pow;
void rtPID(int degreeamount) {
while (done)//This makes the code run infinitely.
{
count = count + 0.1;
if(count>3){
count=0;
Brain.Screen.clearScreen();
Brain.Screen.print(turn_error);
Brain.Screen.newLine();
Brain.Screen.print(turn_derivative);
Brain.Screen.newLine();
Brain.Screen.print(turn_pow);
Brain.Screen.newLine();
}
turn_error = degreeamount - Inertial17.heading(degrees);
turn_derivative = turn_error - turn_Preverror;
turn_Preverror = turn_error;
turn_pow = (turn_error*turn_kP)+(turn_derivative*turn_kD);
turn_Preverror = turn_error;
bool done = true;
if (turn_error<10){
done=false;
break;
Right_Drive.stop();
Left_Drive.stop();
}
Right_Drive.spin(reverse,turn_pow,volt);
Left_Drive.spin(fwd,turn_pow,volt);
wait(15,msec);
}
}
I wanted to print the values every 30 cycles, thats why i have the count variable - it might mess with everything else but I didn’t think i would.