pos_Setpoint overrun

Hi, we are having significant problems with odrive’s position mode. Basically, around 50% of the time when I arm(put into closed loop control) an axis or set a input Pos the axis will blow past the input pos and continue indefinitely. I am using a 24v board V3.6 on 0.5.2, and the latest odrivetool. I am using the CAN interface to change axis states, set control mode and input mode, and set input_pos. CAN commands are sent from a MCP2515 with a baud rate of 500kbps.

We have a suspicion the issue may be noise/encoder related in some way. So I replaced our encoder(AMT102-V) lines with shielded Cat5e, put a ferrite ring on one of the motors, and put a low esr 2200uf Cap on the PSU output. Despite this, the problem persists.

Figure_1
(Erroneous behavior captured on plotter)


(Expected behavior)

(AXIS0 mount)

(Our Odrive (Without the other axis and encoder attached))
IqSetpoint
(1:Iq_measured, 2: Iq_Setpoint, 3:Id_Setpoint, 4:Id_measured)
Expected_Iq
(Expected Iq Behavior)
(1:Iq_measured, 2: Iq_Setpoint, 3:Id_Setpoint, 4:Id_measured)

Any suggestions or help would be greatly appreciated, thanks!