[Alan Anderson]

Are you trying to drive the robot to a specified location? Your P gain might be too high in the "1" case, making it oscillate around the setpoint. This will happen if your wheels are off the ground without any significant load.

Your shooter encoder is obviously configured with 4x decoding. If you change it to 1x (and change the associated constant 1440 to 360), you can get slightly better results, though since you're computing the rate yourself from the position it isn't a big deal. You should definitely see an improvement in shooter speed stability if you change its simple delay-throttled while loop to a timed loop, or if you do a similar delta computation on the actual system millisecond timer instead of just assuming the loop runs at a fixed rate. I think the constant 20 is involved there -- are you commanding the shooter speed in revolutions per second?

In your While loops, I see a consistent pattern of testing for (i+1) being greater than or equal to a constant. Why not just test for i being greater?