This documents the motor-control approach used for the Avatar M5730 and M4210 integrated joint actuators on the robotic arm. It explains how the current standalone control code works and why its control sequence is necessary.

Control Approach

The current implementation uses interpolated position mode (mode 7). This is the mode that produced the best results during testing; other modes tested did not produce comparably good motion.

Rather than issuing one destination and assuming the actuator will handle the entire movement as intending, the code generates a sequence of intermediate position targets and sends them at regular intervals.

This provides the controller with an explicit trajectory to execute and a predictable point at which to update each motor's target.

The current code uses:

• Interpolated Position Mode
• A nominal 20ms motion-update interval
• Position increments of 500 encoder counts per waypoint
• An RPDO1 command followed by a SYNC message for each motion update
• Position feedback to check the result after the movement

These values are tested for a specific scenario, it is not universal limits for every actuator or every future trajectory.

Motor Initialization and Enable Sequence

Before a motor can execute motion commands, the controller must establish the correct operating state.

The current code performs the following operations:

1. Communicate with the motor through CANopen
2. Configure interpolated position mode
3. Execute the tested controlword sequence: 0x0006 → 0x0007 → 0x000F → 0x001F
4. Begin sending position targets only after the initialization procedure

This order matters because a valid motion command does not guarantee that an actuator is configured or enabled to execute it. Initialization should therefore be a distinct stage of the controller rather than code mixed into the trajectory generation loop.

Raspberry Pi Controller Motor States

The full controller should represent each motor's state explicitly. For example, a motor should not be treated as ready to move merely because its CAN interface is available.

A state model:

• Uninitialized: The controller has not completed motor setup
• Initializing: Configuration and enable commands are being executed
• Ready: Initialization has completed and the motor is ready for motion
• Moving: The controller is executing a trajectory
• Fault or Communication Error: The controller has detected an issue requiring attention

Executing a Position Trajectory

For a movement from initial position (p_0) to a target position (p_1), the controller generates a sequence of intermediate position targets between those endpoints. Each target is sent through the configured RPDO1 mapping, followed by SYNC.

1. Determine the starting position
2. Calculate the destination
3. Generate intermediate waypoints
4. Send each waypoint and the corresponding SYNC update
5. Complete the trajectory
6. Read feedback and evaluate the result

This is important because the trajectory generation and motor communication are separate responsibilities. The trajectory determines where the motor should be commanded to go at each update. The communication layer delivers those commands to the actuator.

Position Targets and Reference Positions

The current code reads the initial position once, stores it, commands an outbound movement, and then commands a return to that saved position.

This makes the return target explicit. It avoids unintentionally changing the destination by recalculating it from a new position reading.

The distinction becomes more important in the full controller, where position commands may come from higher-level software. The controller will need a clear convention for whether a command represents an absolute joint position or a relative movement.

Motion Timing and SYNC

The current script uses a nominal 20ms update interval, equivalent to a 50hz update rate. It scheduels updates using a monotonic clock so that execution time does not simply accumulate into every subsequent interval.

For interpolated position control, timing and message order are part of the control procedure. The code sends the position command before SYNC, then waits for the next scheduled update.

The controller must preserve that tested ordering. If the two messages are sent in the wrong order, or updates are delayed inconsistently, the actuator may not process the trajectory as intended.

The code also avoids frequent feedback reads inside the motion-update loop. This keeps the time-sensitive command path simpler and reduces the chance that extra work delays the next update.