From 574a60cbb61b1ad089b02a4bd9bf36c41f121aa1 Mon Sep 17 00:00:00 2001 From: TQ Hirsch Date: Fri, 15 Apr 2022 17:28:01 +0200 Subject: [PATCH] Got the firmware to compile and produce something vaguely sensible. --- firmware/.envrc | 1 + firmware/docs/implementation-notes.adoc | 10 +- firmware/shell.nix | 10 ++ firmware/src/main.rs | 16 ++- firmware/src/motion/planner.rs | 168 +++++++++++++++++++----- 5 files changed, 164 insertions(+), 41 deletions(-) create mode 100644 firmware/.envrc create mode 100644 firmware/shell.nix diff --git a/firmware/.envrc b/firmware/.envrc new file mode 100644 index 0000000..051d09d --- /dev/null +++ b/firmware/.envrc @@ -0,0 +1 @@ +eval "$(lorri direnv)" diff --git a/firmware/docs/implementation-notes.adoc b/firmware/docs/implementation-notes.adoc index 17e5ce9..76e43cd 100644 --- a/firmware/docs/implementation-notes.adoc +++ b/firmware/docs/implementation-notes.adoc @@ -1,6 +1,6 @@ Motion control in an interrupt. -= Parts +== Parts This consists of two parts, the planner and the executor. The planner receives target positions. Each time it receives a target @@ -13,7 +13,7 @@ the step lines of the MCU. These two processes communicate by means of a command queue. -== Executor +=== 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. @@ -21,7 +21,7 @@ These two processes communicate by means of a command queue. -== 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 @@ -40,9 +40,9 @@ A motion profile segment consists of the following values: The following invariants hold for the command queue: -== Planner +=== Planner -=== Aborting +==== 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 diff --git a/firmware/shell.nix b/firmware/shell.nix new file mode 100644 index 0000000..d24f6e5 --- /dev/null +++ b/firmware/shell.nix @@ -0,0 +1,10 @@ +{ pkgs ? import {} }: + +pkgs.mkShell { + buildInputs = [ + pkgs.stdenv + + # keep this line if you use bash + pkgs.bashInteractive + ]; +} diff --git a/firmware/src/main.rs b/firmware/src/main.rs index a08b146..ff0daec 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -2,6 +2,20 @@ pub mod motion { pub mod planner; } +use motion::planner::{Planner, Config}; +use crate::motion::planner::State; + fn main() { - println!("Hello, world!"); + let planner_config = Config { + j_max: 231., + v_max: 0.0, + a_max: 100. , // WAG + step_size: 0.2 + }; + let planner = Planner::new(5_000, planner_config) + .expect("Planner config should succeed"); + + let profile = planner.plan_profile(planner.step_size() as i64 * 2500, State::default()); + println!("Profile: {:#?}", profile); + } diff --git a/firmware/src/motion/planner.rs b/firmware/src/motion/planner.rs index f1d380f..482f570 100644 --- a/firmware/src/motion/planner.rs +++ b/firmware/src/motion/planner.rs @@ -1,9 +1,18 @@ +//! The motion planner. +//! +//! Note that the equations in this file are rather complex and non-obvious. +//! All of their derivations can be found in the motion-control.ipynb file. + +use std::num::Wrapping; + pub struct Config { - j_max: f32, // in mm/s^3 - v_max: f32, // mm/s^2 - // mm/s - a_max: f32, - step_size: f32, // um ! + /// Max jerk, in mm/s^3 + pub j_max: f32, + /// Max velocity, in mm/s. If 0, 1 step every other tick. + pub v_max: f32, // mm/s + // Max acceleration, in mm/s^2. + pub a_max: f32, + pub step_size: f32, } #[derive(Debug, Clone)] @@ -13,11 +22,15 @@ pub struct Planner { // the appropriate power of time for the unit. v_max: u32, a_max: u32, + rt2_a_max: u32, // square root of a_max step_size: u32, // nsteps for each regime - xmax_cj: u32, - xmax_ca: u32, + xmax_cj: u64, + xmax_ca: u64, + + tj_max: u32, + ta_max: u32, } #[derive(Copy, Clone, Debug)] @@ -25,10 +38,10 @@ pub struct Profile { segments: [Segment; 7] } -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, Default)] pub struct Segment { // Used by executor - pub delta: [u32; 3], + pub delta: [i32; 3], start_time: u32, // 0 to disable; set after completion. // used by planner v0: i32, @@ -37,9 +50,30 @@ pub struct Segment { } impl Segment { - pub fn state_at_time(&self, time: u32) -> State { - let j = self.delta[2] as i32; - let + // This will work for |t|<=65535 + pub fn state_at_time_u64(&self, time: u32, step_size: u32) -> (i32, State) { + let j = self.delta[2] as i32; // 6, 0, or -6 + let t = time as i64; + let t2 = (time * time) as i64; + // TODO: figure out what can be handled as u32, as 64-bit arithmentic is significantly slower + let dp = (j / 6) as i64 * t * t2 + (self.a0 / 2) as i64 * t2 + self.v0 as i64 * t; + let dv = (j / 2) * (t2 as i32) + self.a0 * time as i32; + let da = j * time as i32; + let time = self.start_time + time; + let pe = self.p0 as i64 + dp; + + let p0 = pe.rem_euclid(step_size as i64) as u32; + let nstep = pe.div_euclid(step_size as i64) as i32; + + + let new_state = State { + time: self.start_time + time, + p0, + a0: self.a0 + da, + v0: self.v0 + dv, + }; + + (nstep, new_state) } } @@ -53,11 +87,15 @@ pub struct State { impl State { fn segment_for(&self, j: i32) -> Segment { + let a0_2 = Wrapping((self.a0 / 2) as u32); + let a0 = Wrapping((self.a0) as u32); + let j_6 = Wrapping((j / 6) as u32); + let j_2 = Wrapping((j / 2) as u32); Segment { delta: [ - (self.a0 / 2) as u32 + (j / 6) as u32 + self.v0 as u32, - self.a0 as u32 + j as u32, - j as u32, + ((self.a0 / 2) + (j / 6) + self.v0), + self.a0 + j, + j, ], start_time: self.time, v0: self.v0, @@ -66,12 +104,16 @@ impl State { } } - fn produce_segment(&mut self, j: i32, length: u32) -> Segment { + fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Segment { let segment = self.segment_for(j); - let t = length; - let t2 = t * length; - let t3 = t2 * length; - self.p0 += (j / 6) as u32 * t3 + (self.a0 / 2) as u32 * t2 + self.v0 as u32 * t; + let t = length as i32; + let t2 = t * length as i32; + let t3 = t2 as i64 * length as i64; + let dp = (j / 6) as i64 * t3 + + (self.a0 / 2) as i64 * t2 as i64 + + self.v0 as i64 * t as i64; + + self.p0 = (self.p0 as i64 + dp).rem_euclid(step_size as i64) as u32; self.v0 += j / 2 * t2 as i32 + self.a0 * t as i32; self.a0 += j * t as i32; @@ -89,7 +131,12 @@ impl Planner { tick_frequency, v_max: 0, a_max: 0, + rt2_a_max: 0, step_size: 0, + xmax_cj: 0, + xmax_ca: 0, + tj_max: 0, + ta_max: 0 }; if ret.reconfigure(config) { @@ -104,36 +151,87 @@ impl Planner { pub fn reconfigure(&mut self, config: Config) -> bool { let tick_rate = self.tick_frequency as f32; let a_max = (6. * config.a_max * tick_rate / config.j_max) - .clamp(0.0, (1 << 32) as f32); - let v_max = (6. * config.v_max * tick_rate * tick_rate / config.j_max) - .clamp(0.0, (1 << 31) as f32); - let step_size = config.step_size * 6. * tick_rate * tick_rate * tick_rate / config.j_max / 1000.; + .clamp(0.0, (1u32 << 31) as f32); + + let v_max = if config.v_max == 0. { + config.step_size * tick_rate / 2. + } else { + config.v_max + }; + + let v_max = (6. * v_max * tick_rate * tick_rate / config.j_max) + .clamp(0.0, (1u32 << 31) as f32); + let step_size = config.step_size * 6. * tick_rate * tick_rate * tick_rate / config.j_max; if step_size > (u32::MAX / 2) as f32 { + eprintln!("Failed to configure planner: stepsize = {}", step_size); return false; } self.a_max = a_max as u32; + self.rt2_a_max = a_max.sqrt() as u32; self.v_max = v_max as u32; self.step_size = step_size as u32; + // Compute ta_max and tj_max + self.tj_max = self.a_max / 6; + self.ta_max = self.v_max / self.a_max - self.tj_max; + // Compute regime change points - let amax2 = self.a_max * self.a_max; - self.xmax_cj = self.a_max * amax2 / 18; // jmax = 6, xmax_cj = 2*amax^3/jmax^2 - self.xmax_ca = self.a_max * self.v_max / 6 + self.v_max * self.v_max / self.a_max; + let a_max_2 = self.a_max as u64 * self.a_max as u64; + self.xmax_cj = self.a_max as u64 / 18 * a_max_2; // jmax = 6, xmax_cj = 2*amax^3/jmax^2 + self.xmax_ca = self.a_max as u64 * self.v_max as u64 / 6 + self.v_max as u64 * self.v_max as u64 / self.a_max as u64; + return true; } - pub fn plan_profile(&self, dx: i32, state: State) -> Profile { + // Note that dx is in internal units + pub fn plan_profile(&self, dx: i64, state: State) -> Profile { let mut cstate = state; - let (tj, ta, tv) = - if dx.abs() as u32 <= self.xmax_cj { - - }; + let j = 6; + let dir = dx.signum() as i32; + let dx = dx.abs() as u64; + + + let (tj, ta) = + if dx <= self.xmax_cj as u64 { + let tj = f32::cbrt(dx as f32 / 2. / j as f32) as u32; + (tj, 0) + } else if dx <= self.xmax_ca as u64 { + let ta_quadrat = + self.a_max * self.a_max / (4 * 36) // amax^3/(4*amax*jmax^2) + + (dx / self.a_max as u64) as u32; + let ta = f32::sqrt(ta_quadrat as f32) as u32 + - self.a_max / 4; + (self.tj_max, ta) + } else { + (self.tj_max, self.ta_max) + }; + + // Now we have a value for t_j and t_a. Compute the velocity and necessary Δx during t_v + let s4v = j * tj * (ta + tj); + let ramp_dx = (j * tj) as u64 * (ta * ta + 3 * ta * tj + 2 * tj * tj) as u64; + let tv = (dx - ramp_dx) * 2 / s4v as u64; + let tv = ((tv + 1) / 2) as u32; + + let j_real = dir * 6; + let mut segments = [Segment::default(); 7]; + let seg_params = [ + (j_real, tj), + (0, ta), + (-j_real, tj), + (0, tv), + (-j_real, tj), + (0, ta), + (j_real, tj), + ]; + for (i, (j,t)) in seg_params.iter().copied().enumerate() { + segments[i] = cstate.produce_segment(j, t, self.step_size); + } Profile { - segments: [ - Planner::segmentFor() - ] + segments, } } + pub fn step_size(&self) -> u32 { self.step_size } + } \ No newline at end of file