Sc mc python examples
From DIDEAS Wiki
prepare the hardware
Take the robot out of its boot mode so that it can be controlled externally. Generally the following special commands achieve this:
- [231, 0] # IMU off - to minimize possible loss of commands over the FUP
- [173,77,7] # demux statemachine to idle mode
- [170,77,17] # primary statemachine to the 'do nothing state'
- [1421, 17] # MC statemachine to a 'do nothing success state'
move robot back - fort in voltage mode
After preparing the hardware above, can use special command 1115 to command a fixed voltage across the motor:
- [1115, 1000, 0, 6] # qVq = 1000, mode = 6 (voltage
- sleep 1 sec
- [1115, -1000, 0, 6] # qVq=-1000
- sleep 1 sec
- [1115, 0,0,0] # short the motor leads
enter trajectory mode
The trajectory mode will servo the robot to the goal position. This is achieved by adjusting the sent point of a virtual spring (whos parmeters) are adjusted. The goal point for the VS is set based on diffeence equstion that produces an exponential response to a position step input.
- [1318, 100] # set goal position to 100 need to be careful to limit the goal position to valid positions!!!
- [1421, 41] # go to trajectory control mode, robot immediatly jumps!
- sleep 1 sec
- [1318, 500] # servo to position 500