64 lines
1.9 KiB
Plaintext
64 lines
1.9 KiB
Plaintext
Motion control in an interrupt.
|
|
|
|
== Parts
|
|
This consists of two parts, the planner and the executor.
|
|
|
|
The planner receives target positions. Each time it receives a target
|
|
position, it replans so as to reach that position as soon as possible;
|
|
the output of a plan consists of a set of motion segments, each
|
|
following a 3rd order polynomial.
|
|
|
|
The executor processes the current state and decides whether to toggle
|
|
the step lines of the MCU.
|
|
|
|
These two processes communicate by means of a command queue.
|
|
|
|
=== Executor
|
|
1. Update a cycle counter
|
|
2. Evaluates the next output of the position polynomial (3 adds)
|
|
3. determine whether to toggle a stepper, and do so.
|
|
4. Determine whether to start the next segment; if so, advance in the command queue.
|
|
|
|
|
|
|
|
=== Command queue
|
|
|
|
The command queue takes the form of a ring buffer, with each item
|
|
containing a motion segment. The ring buffer must be large enough to
|
|
contain a complete motion profile plus a terminating "stop" segment.
|
|
|
|
A motion profile segment consists of the following values:
|
|
|
|
- For step computation
|
|
* Δx1..Δx3 (initial values)
|
|
* start time (in ticks)
|
|
- For use by the planner
|
|
* v₀: Initial velocity
|
|
* a₀: Initial acceleration
|
|
* The actual position at the start time (written by executor)
|
|
|
|
The following invariants hold for the command queue:
|
|
|
|
|
|
=== Planner
|
|
|
|
==== Aborting
|
|
|
|
In case of an abort, the fastest stop profile will consist of at most
|
|
3 segments: const -jerk to max -a, const -a to to lead-out, const +j
|
|
to a=v=0.
|
|
|
|
If we're already in the last three segments, we're already in
|
|
a race to a stop, so there's no need to handle an abort specially.
|
|
|
|
In the cv segment, we can simply advance the start times of each leadout segment.
|
|
In the lead-in, we need to replan; this is, however, easy enough:
|
|
|
|
1. Predict state in $t_plan$ cycles
|
|
2. Identify how to reach a=0; prepare a segment accordingly
|
|
3. Feed the end values from said segment to the leadout calculator
|
|
4. replace the next N commands with this leadout.
|
|
|
|
|
|
|