Hello, I haven’t had time to tune the kp ki and kd values, but this is my first time doing PID as a programmer and I need to know if this code would work for maintaining the speed of a two-motor flywheel.
motor_group Flywheel = motor_group(Fly1, Fly2);
bool maintainSpeed = false;
double targetspeed = 600;
double kp = 0.5;
double ki = 0.10;
double kd = 0.05;
double preverror = 0.0;
double error = 0.0;
double totalError = 0.0; // += error
double derivative = 0.0; // = error-preverror
double flyspeed;
double Power = 0;
bool ReadyShoot = false;
int FlyPID(){
while(maintainSpeed){
flyspeed = (Fly1.velocity(rpm) + Fly2.velocity(rpm))/2;
error = targetspeed - flyspeed;
if (error <= 0.1){
ReadyShoot = true;
}
else{
ReadyShoot = false;
}
Power += (error*kp + totalError * ki + (error - preverror) * kd)/12;
Flywheel.spin(forward, Power, volt);
preverror = error;
totalError += error;
vex::task::sleep(20);
}
return 1;
}