Flywheel PID Code

[Ryan_Zheng]

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;
}

[EcstaticPilot]

Not entirely sure about this but if maintainSpeed is false the function might skip over the while loop ending the task. Also, power should be an = instead of +=.

[Aarakei]

Motor rpm readings have a fair bit of noise, so I doubt that consistently having an error less than 0.1 rpm would be realistic. I would also make sure to tell the flywheel to coast after exiting the loop.