Currently, my team is trying to have our robot turn much faster and more accurate based on the inertial sensor. We’ve gotten it to turn to a certain degree with the inertial sensor, but due to no change in velocity or correction it overshoots or spins in circles continuously. Thats when I considered using a PID to help fix our problem.
I was wondering based on the code below if gryo Pid turning would work with my teams robot. I found this online and adapted what I could to Vex V5, but I currently can’t test this on any robot. If this code simply doesn’t work, I would also appreciate the feedback.
void gyroPid (double angle) {
double threshold;
if(angle <= 0.0) {
threshold = 1.5;
}
else {
threshold = 0.7;
}
//variable instantiations
double error = angle - Inertial11.rotation();
double integral;
double derivative;
double prevError;
double kp = 0.0;
double ki = 0.0;
double kd = 0.0;
while(fabs(error) > threshold) {
error = angle - Inertial11.rotation();
integral = integral + error;
if(error == 0 || fabs(error) >= angle) {
integral = 0;
}
derivative = error - prevError;
prevError = error;
double p = error * kp;
double i = integral * ki;
double d = derivative * kd;
double vel = p + i + d;
Leftside.setVelocity(vel, percent);
Rightside.setVelocity(-vel, percent);
Leftside.spin(forward);
Rightside.spin(forward);
}
}