const float kp = 2;
const float ki = 0.5;
const float kd = 2;
float targetFlywheelSpeedrpm = 500; // put desired rpm flywheel speed here
float desiredMilliVoltage = 10000;
float flywheelVelocityrpm;
float error = targetFlywheelSpeedrpm - flywheelVelocityrpm;
float lastError = 0;
float totalError = 0;
float finalAdjustment = error * kp;
float flywheelPowerVolts;
int autonPIDFlywheel() {
while (1) {
if (wantToShoot) {
flywheelVelocityrpm = (flywheel7.velocity(rpm) + flywheel8.velocity(rpm)) / -2;
error = targetFlywheelSpeedrpm - flywheelVelocityrpm;
totalError += error;
printf("error: %f \n", error);
printf("Flywheel: %f \n", (flywheel7.velocity(rpm) + flywheel8.velocity(rpm)) / - 2);
if (std::abs(error) <= 5) { // 0-5 noise rpm?
readyToShoot = true;
Controller1.rumble("-");
printf("HERE HERE HERE: %f \n", (flywheel7.velocity(rpm) + flywheel8.velocity(rpm)) / - 2);
}
else {
readyToShoot = false;
}
flywheelPowerVolts = ((error * kp) + (totalError * ki) + ((error - lastError) * kd))/12;
flywheelPowerVolts += (desiredMilliVoltage / 1000);
flywheel7.spin(reverse, flywheelPowerVolts, volt);
flywheel8.spin(reverse, flywheelPowerVolts, volt);
lastError = error;
}
else {
flywheel7.stop(coast);
flywheel8.stop(coast);
}
this_thread::sleep_for(20);
}
return 0;
}
Had this issue before; is there any way for flywheel pid motors to not break when it is below 600 rpm? Thanks!