Fixes issue causing initial jerk/kick with multiple motors on the sam…

…e bus.

README.md

@@ -176,6 +176,6 @@ To add a new constants configuration use the `change_motor_constants` function o
 
 # Known Issues
 
-When having 2 motors on the CAN bus with either PCAN CAN-USB or ESD CAN-USB/2, sometimes the motors experience an initial short *kick/impulse* at when they are enabled again after being disabled. One workaround is power cycling them. This is probably due to separate grounds for the power and CAN communication on the motors. As the ground for the CAN can be via the control computer ground and the power ground is via the power supply ground, these can have small voltage differences which can cause the initial kick. This is the current best guess for when experiencing the issue. 
+**Issue Fixed**: When having 2 motors on the CAN bus with either PCAN CAN-USB or ESD CAN-USB/2, sometimes the motors experience an initial short *kick/impulse* at when they are enabled again after being disabled. This was fixed.
 
-As this is experimental software, there might be other unknown issues. 
+As this is experimental software, there might be other unknown issues.

src/motor_driver/canmotorlib.py

@@ -289,6 +289,14 @@ def disable_motor(self):
         Sends the disable motor command to the motor.
         """
         try:
+            # Bugfix: To remove the initial kick at motor start.
+            # The current working theory is that the motor "remembers" the last command. And this
+            # causes an initial kick as the motor controller starts. The fix is then to set the 
+            # last command to zero so that this does not happen. For the user, the behavior does
+            # not change as zero command + disable is same as disable.
+            _, _, _ = self.send_rad_command(0, 0, 0, 0, 0)
+
+            # Do the actual disabling after zero command.
             self._send_can_frame(b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFD')
             waitOhneSleep(dt_sleep)
             can_id, can_dlc, motorStatusData = self._recv_can_frame()