Joints
(TODO: describe joints, axes, URDF, and calibration, and how this relates to servos)
Periodic Servo Scanner
Periodically scans for connected servos. Once a new bus is found that isn't already used, and the servo IDs match with one of the joint sets, the bus is attached to that joint set, and the servos will start to move.
The Servo Loop
Updates the joints in all joint sets. The update cycle for a joint is:
-
receive from servo - In the previous iteration, the servos were requested to return status information for position, speed, voltage, etc. Here the core collects the responses from the servos.
-
process responses - The response data is decoded into servo states, and the servo positions are interpreted according to the joint type.
-
send new goals - Current goal positions/speeds/torque are encoded and sent to the servos.
-
request from servo - Finally, the servos are requested to return status information for position, speed, voltage, etc. which will be processed in the next iteration.