Shooter PD loop

I just learned about PID loops a week ago and wanted to use it for our flywheel.
I’m not sure if my current code works correctly so I’d like people to check for me.
Our flywheel uses two 600 rpm motors

I am not completely sure but I think It would work just make sure to test it out before you take my word on it.

My comment: you only have a P loop here, based on how the code is executing.

In your forever loop, you have set Pd to zero, meaning that whatever your D value is doesn’t matter.

Yah I just noticed that too and fixed it, set Kd to 0.6

2 things are wrong with this

  1. You are setting the velocity but not telling the motor to spin. The part where you are is beneath a forever loop that will never exit.
  2. A straight pd loop is not a good idea for a flywheel because when it goes over the target speed, the error will become negative meaning the speed will also be negative. A way around this is called feedforward which adds the target speed to the output of the pd. Because you know that it needs to go a target speed and you are seeing the velocity, you set the velocity to the target speed + pd so that if error is zero or negative it tries to go that speed by setting that as a imaginary zero point. This is because in a typical PID is for position, not velocity and if speed is 0 in a positional case, the error doesn’t increase.