The PID i have programed to turn with the inertial sensor is behaving oddly, and I have no real idea why. Is anyone able to tell me where I’ve gone wrong?
void turning_pid(int target, int mode = 0, float tolerance = 0.1){
float kP = 0.5;
float kI = 0.1;
float KD = 0.4;
float P;
float I;
float aI;
float D;
float aD;
float value;
float power;
float lasterror;
bool firstloop = 1;
bool left_negative;
//true = left, false = right
bool turning_direction = true;
while(1){
if(mode == 0){
value = field_imu.get_heading();
}
else if(mode == 1){
value = turning_imu.get_heading();
}
float error = target - value;
//for right turns
if(error < 0){
error = (-error);
if(error > 180){
error = (360-value) + target;
}
else{
error += 180;
}
}
if(firstloop){
turning_direction = error < 180;
firstloop = 0;
}
P = error * kP;
I = I + error;
aI = I * kI;
D = error - lasterror;
aD = D * KD;
power = P + aI + aD;
std::cout << error << std::endl;
if(turning_direction){
left_mtrs = !power;
right_mtrs = power;
if(error < tolerance){
left_mtrs.brake();
right_mtrs.brake();
if(mode == 1){
turning_imu.set_heading(0);
}
break;
}
}
else if(!turning_direction){
right_mtrs = !power;
left_mtrs = power;
if((error - 180) < tolerance){
left_mtrs.brake();
right_mtrs.brake();
if(mode == 1){
turning_imu.set_heading(0);
}
break;
}
}
lasterror = error;
pros::delay(20);
}
std::cout << "end" << std::endl;
}
Thanks in advance