diff --git a/firmware/film-scanner-fw/.cargo/config.toml b/firmware/film-scanner-fw/.cargo/config.toml index 774199e..49f739b 100644 --- a/firmware/film-scanner-fw/.cargo/config.toml +++ b/firmware/film-scanner-fw/.cargo/config.toml @@ -20,7 +20,7 @@ rustflags = [ ] [build] -target = "thumbv7em-none-eabihf" +target = "thumbv6m-none-eabi" [env] DEFMT_LOG = "debug" diff --git a/firmware/film-scanner-fw/src/main.rs b/firmware/film-scanner-fw/src/main.rs index 2f29023..63fad3a 100644 --- a/firmware/film-scanner-fw/src/main.rs +++ b/firmware/film-scanner-fw/src/main.rs @@ -15,8 +15,10 @@ mod app { use core::num::Wrapping; use embedded_hal::digital::v2::OutputPin; use embedded_time::duration::Extensions; + use jerk_control::executor::CommandQueue; + use jerk_control::planner::Planner; - use rp_pico::hal; + use rp_pico::hal::{self, gpio::pin::{Output, PushPull, PullDownDisabled, Pin}}; use rp_pico::pac; use rp_pico::XOSC_CRYSTAL_FREQ; use rtic::mutex_prelude::{TupleExt04, TupleExt05}; @@ -69,11 +71,26 @@ mod app { pub active: bool, } - pub struct StepperProcess { + pub struct StepperProcess + where StepPin: rp2040_hal::gpio::PinId, + DirPin: rp2040_hal::gpio::PinId { pub executor: executor::CommandQueue, - pub pin: Pin, + pub step: rp_pico::hal::gpio::Pin>, + pub dir: rp_pico::hal::gpio::Pin>, } + impl StepperProcess + where StepPin: rp2040_hal::gpio::PinId, + DirPin: rp2040_hal::gpio::PinId { + pub fn new(step: Pin, dir: Pin) -> Self { + Self { + executor: CommandQueue::new(), + step: step.into_push_pull_output(), + dir: dir.into_push_pull_output(), + } + } + } + #[shared] struct Shared { @@ -89,9 +106,11 @@ mod app { } + use rp2040_hal::gpio::pin::bank0; + #[local] struct Local { - executor1: StepperProcess>, + executor1: StepperProcess, } #[init(local = [usb_bus: Option> = None])] @@ -178,7 +197,9 @@ mod app { serial, usb_dev, }, - Local {}, + Local { + executor1: StepperProcess::new(pins.gpio1, pins.gpio2), + }, init::Monotonics(), ) } @@ -188,9 +209,24 @@ mod app { binds = TIMER_IRQ_0, priority = 4, shared = [timer, alarm, serial], - local = [tog: bool = true], + local = [executor1], )] fn timer_irq(mut _cx: timer_irq::Context) { + let executor: &mut StepperProcess<_, _> = _cx.local.executor1; + + if executor.executor.direction() { + executor.dir.set_high(); + } else { + executor.dir.set_low(); + } + + if executor.executor.step_early() { + executor.step.set_high(); + } else { + executor.step.set_low(); + } + + executor.executor.step_late(); } /// Usb interrupt handler. Runs every time the host requests new data. diff --git a/firmware/jerk_control/src/executor.rs b/firmware/jerk_control/src/executor.rs index 532a68e..3be023b 100644 --- a/firmware/jerk_control/src/executor.rs +++ b/firmware/jerk_control/src/executor.rs @@ -36,7 +36,7 @@ impl CommandQueue { // Returns true if step signal should be high. This does the first, constant time part of stepping. // Note that step_late needs to be called as well to advance the segment. pub fn step_early(&mut self) -> bool { - + let cur_segment = &mut self.segments[self.cur_segment as usize]; self.p.0 = (self.p + cur_segment.delta[0]).0.rem_euclid(self.step_size); diff --git a/firmware/jerk_control/src/lib.rs b/firmware/jerk_control/src/lib.rs index d2f90d3..6c08869 100644 --- a/firmware/jerk_control/src/lib.rs +++ b/firmware/jerk_control/src/lib.rs @@ -1,4 +1,4 @@ -// #![no_std] +#![no_std] pub mod planner; pub mod executor; diff --git a/firmware/jerk_control/src/math_ext.rs b/firmware/jerk_control/src/math_ext.rs index b7d7924..1e9b890 100644 --- a/firmware/jerk_control/src/math_ext.rs +++ b/firmware/jerk_control/src/math_ext.rs @@ -41,12 +41,22 @@ fn sqrt_approx(value: f32) -> f32 { pub fn cbrt_approx(value: f32) -> f32 { let value = value.to_bits(); - const EXP_BIAS: i32 = 0x3f80_0000; + //const EXP_BIAS: i32 = 0x3f80_0000; f32::from_bits((value & 0x8000_0000) | - (((value as i32 & 0x7fff_ffff) - EXP_BIAS) / 3 + EXP_BIAS) as u32) + (((value & 0x7fff_ffff) / 3).wrapping_add(0x2a5119f2))) } +fn frexp(value: f32) -> (i32, f32) { + let value = value.to_bits(); + (((value >> 23) & 0xff) as i32 - 126, // we go to [0.5,1), whereas the mantissa represents [1,2) + f32::from_bits(value & 0x7f_ffff | (126 << 23))) +} +fn ldexp(exp: i32, mantissa: f32) -> f32 { + f32::from_bits(mantissa.to_bits().wrapping_add((exp as u32) << 23)) +} + + impl Roots for f32 { fn sqrt(self) -> Self { let mut estimate = sqrt_approx(self); @@ -56,16 +66,18 @@ impl Roots for f32 { estimate } + //#[cfg(ignore)] fn cbrt(self) -> Self { - let mut estimate = cbrt_approx(self); // TODO: improve this with bit twiddling - + let mut estimate = cbrt_approx(self) as f64; // TODO: improve this with bit twiddling + let a = self as f64; + // Use halley's method to refine the approximation. - for _ in 0..3 { + for _ in 0..2 { let e3 = estimate * estimate * estimate; - estimate = estimate * (e3 + self + self) / ( e3 + e3 + self); + estimate = estimate * (e3 + a + a) / ( e3 + e3 + a); } - estimate + estimate as f32 } } diff --git a/firmware/jerk_control/src/planner.rs b/firmware/jerk_control/src/planner.rs index f413e9c..51b66ee 100644 --- a/firmware/jerk_control/src/planner.rs +++ b/firmware/jerk_control/src/planner.rs @@ -5,6 +5,7 @@ use core::num::Wrapping; use core::option::Option; +use crate::math_ext::Roots as _; pub struct Config { /// Max jerk, in mm/s^3 diff --git a/firmware/motion-tester/src/bin/math_test.rs b/firmware/motion-tester/src/bin/math_test.rs index 913f080..8033f7e 100644 --- a/firmware/motion-tester/src/bin/math_test.rs +++ b/firmware/motion-tester/src/bin/math_test.rs @@ -5,43 +5,30 @@ fn ulp(f: f32) -> f32 { } fn fn_err(actual: f32, calc: f32) -> f32 { - (calc - actual).abs() / actual + (calc - actual).abs() / ulp(actual) } fn test_all(fname: &str, actual: fn(f32) -> f32, calc: fn(f32) -> f32) { let mut max_err = 0f32; let mut max_err_value = 0f32; - (0x01..=0xfe).into_par_iter() + let errors = + (0x01..=0xfe).into_par_iter() .map(|exp| { - (0x0..=0x7f_ffff).map(|mant| f32::from_bits((exp << 23) + mant)) + let max_err_place = (0x0..=0x7f_ffff).map(|mant| f32::from_bits((exp << 23) + mant)) .max_by_key(|value| fn_err(actual(*value), calc(*value)).to_bits()) - .unwrap() + .unwrap(); + (exp, max_err_place, fn_err(actual(max_err_place), calc(max_err_place))) }) - .map(|value| (value,fn_err(actual(value), calc(value)))) .collect::>(); - for i in (0x0400_0000..0x0f7f_ffffu32).map(f32::from_bits) { - // range of normalized floats... - let err = fn_err(actual(i), calc(i)); - if err > max_err { - max_err = err; - max_err_value = i; - } + for (exp, base, err) in errors { + println!("{},{},{}", exp as i32 - 127,base, err); } - - let max_err_pct = max_err * 100f32; - println!("{}: {}% @ {}", fname, max_err_pct, max_err_value); - println!( - "Value: 0x{:08x}\n - Calculated: 0x{:08x}\n - Actual: 0x{:08x}", - max_err_value.to_bits(), - calc(max_err_value).to_bits(), actual(max_err_value).to_bits()) } fn main() { - // test_all("sqrt", f32::sqrt, jerk_control::math_ext::Roots::sqrt); + //test_all("sqrt", f32::sqrt, jerk_control::math_ext::Roots::sqrt); test_all("cbrt", f32::cbrt, jerk_control::math_ext::Roots::cbrt); -} \ No newline at end of file +}