We use python and robotpy to control an arm with a pid loop. This pid gets continuously different setpoints based on the angle that the arm is at and the angle that it should be at. (These functions are on the scheduler branch in minimal_drivecode/subsystems/arm.py) The arm has two problems. The first is that if enabled, ran an arm command, moved the arm, disabled then re-enabled, upon the re-enable the arm will automatically move unless the command had been interrupted by the command do_die_you_gravy_sucking_pig during the first movement. The second problem – if you all incidentally happen to catch it – is that the arm goes to a roughly 120 degree angle when told to go to 60. Moreover if told to go to 135 degrees, the arm will oscillate at 0 degrees.
To my knowledge disabling the robot does not cancel any commands. It stops sending any output signals to motors and such, but the commands continue to run.
Is there a quick way to ensure when disabled all commands are cancelled?
I believe the following in your
disabledInit() method should do the trick:
If commands are not set to run when disabled, they will be canceled when disabled. Are you turning things off in the interrupted and end methods?