Hey @digitalnomadjoe,
I would suggest you to use the motor commands and rtune the parameters with it. It will make the iterations faster.
You need to tune 5 parameters. Velocity pid controller’s PID gain, filtering constant Tf of the LPF filter and the angle P gain.
My suggestion would be.
- for now keep the D terms at 0
- try playing with the filtering constant, sometimes large filtering constants such as 0.01 cause oscillating. Lower it to 0.001-0.005.
- try lowering the P gain of the angle controller.
- the reason for the spikes is a large I term of the pid. You could set it to 0 once you reach the target, or to some other small value. But investigate also the filtering constant Tf, it might me a filtering issue.