Motion Planning

Understanding and creating custom folding sequences

The Motion Plan Structure

A Motion Plan defines exactly what the robot does when a button is pressed. It is a list of Steps, and each Step contains a list of Channels (Servos) to move.

Hierarchy

Button (0-3): Selects which Plan to run
↳ Plan: A list of sequential Steps
↳ Step: A list of Motor Channels (0-15)

Execution Logic

Steps run Sequentially: Step 1 finishes before Step 2 starts.
Channels run in Parallel: All motors in a single Step move together.
Automatic Reset: After the last Step, all used motors return to 0°.

Example Plans

These examples show how to define motion plans in config.py.

Sequential Folding (Standard)

Fold left side (Motor 0), then right side (Motor 1).

[
  [0],  # Step 1: Move Motor 0
  [1]   # Step 2: Move Motor 1 (after Step 1 finishes)
]

Parallel Folding (Simultaneous)

Fold both sides (Motor 0 and Motor 1) at the exact same time.

[
  [0, 1]  # Step 1: Move Motor 0 AND Motor 1 together
]

Complex Sequence

Fold sides (0,1) together, then bottom (2), then top (3).

[
  [0, 1],  # Step 1: Sides
  [2],     # Step 2: Bottom
  [3]      # Step 3: Top
]

The "Flip" Motion

The ServoController implements a specific hardcoded motion whenever a channel is activated. This ensures a consistent folding action.

Start

0° (Home)

Flip Up

Move to 180°

Return

Move back to 10°

Note: The final return to 0° happens only after the entire sequence (all steps) is finished. The intermediate return to 10° prevents the panel from blocking subsequent folds while keeping it out of the way.

How to Edit Plans

Method 1: config.py

Best for permanent, complex plans.

Open config.py in Thonny.
Locate the MOTION_PLANS list.
Edit the list corresponding to the button you want to change (Index 0 = Button 1).
Save the file and reset the Pico.

Method 2: Config Mode

Best for quick, on-the-fly changes without a computer.

Long Press (3s) a button to enter Config Mode (LED turns ON).
Press the button N times to add Motor N to the current step.
Wait 5s to save the step and start a new one (LED blinks).
Wait 10s to save the entire plan and exit (LED turns OFF).