Do your mechanical design imply the motor bearing the cannon weight when it is off vertical? (I’ll assume yes)

Then you’ll want to add a feed forward term to your PID that will be function of the current angle of the cannon and compensate for the weight.

PID work well if the system respond in a linear manner, which means, the same output produce the same result whatever the current system state is. In your case, the cannon is non-linear because if it’s tilted, it’s very much easier to go down (with gravity) than up (against gravity).

The feed forward is computed to cancel gravity and produce a linear response (as long as you are not moving too fast). In example, you’ll want something like:

```
outputSpeed = ... + HORIZONTAL_OUTPUT * sin(shooter_angle)
```

Here HORIZONTAL_OUTPUT is the outputSpeed needed to hold the cannon level while it’s horizontal (where gravity have the more effect) with all the other gains at 0. The sine function will then scale this output between fully vertical (angle 0, so sin(0) = 0.0) and fully horizontal (angle 90, so sin(90) = 1.0).

You should then be able to lower your other gains because they won’t be working so hard anymore be able to tune your PID much more easily.

Be careful your motor is able to stay stalled during a whole match (and tests) because I had incident of motor burning up from holding a static load with insufficient gear ratio.

I would also recommend you move your setpoint slowly toward the desired position instead of directly. A steady slope from 0 to -50 for example will lead to a far better response then setting the target to -50. If the setpoint is too far from the current position, the error is huge and the mechanism will move too quickly and overshoot the target. Lower the gain and it will always be off because low error will not be enough to produce sufficient output to reach the target. I see you are using FPGA time, so you could compute a simple slope with that and use this slope as a target.