Added motion test tool

firmware/jerk_control/src/executor.rs

@@ -0,0 +1,66 @@
+use std::cmp::Ordering;
+use std::num::Wrapping;
+use crate::planner::{Profile, Segment};
+
+#[derive(Default, Debug)]
+pub struct CommandQueue {
+    cur_segment: u8,
+    cur_time: Wrapping<u32>,
+    p: Wrapping<i32>,
+    step_size: i32,
+
+    segments: [Segment; 8],
+}
+
+
+impl CommandQueue {
+    pub fn new() -> Self {
+        CommandQueue::default()
+    }
+
+    pub fn set_step_size(&mut self, step_size: u32) {
+        self.step_size = step_size as i32;
+    }
+
+    pub fn set_profile(&mut self, profile: &Profile) {
+        self.cur_segment = 0;
+        self.segments[0..8].copy_from_slice(profile.segments.as_slice());
+        self.cur_time = Wrapping(0);
+    }
+
+    pub fn running(&self) -> bool {
+        self.segments[self.cur_segment as usize].delta.iter().any(|d| d.0 != 0)
+    }
+
+    // Returns true if step signal should be high
+    pub fn step(&mut self) -> bool {
+        let next_seg_idx = (self.cur_segment + 1) % self.segments.len() as u8;
+        let next_seg_start = self.segments[next_seg_idx as usize].start_time;
+
+        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);
+
+        cur_segment.delta[0] += cur_segment.delta[1];
+        cur_segment.delta[1] += cur_segment.delta[2];
+
+        // Advance the segment if necessary.
+        self.cur_time += Wrapping(1);
+        if self.cur_time.0 >= next_seg_start && next_seg_start != 0 {
+            self.cur_segment = next_seg_idx;
+            cur_segment.start_time = 0;
+        }
+
+
+        // return state of step line
+        return self.p.0 < self.step_size / 2;
+    }
+
+    pub fn direction(& self) -> bool {
+        self.segments[self.cur_segment as usize].delta[0].0 >= 0
+    }
+
+    pub fn time(&self) -> u32 {
+        self.cur_time.0
+    }
+}

firmware/jerk_control/src/lib.rs

@@ -0,0 +1,2 @@
+pub mod planner;
+pub mod executor;

firmware/jerk_control/src/main.rs

@@ -1,21 +0,0 @@
-pub mod motion {
-    pub mod planner;
-}
-
-use motion::planner::{Planner, Config};
-use crate::motion::planner::State;
-
-fn main() {
-    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);
-
-}

firmware/jerk_control/src/planner.rs

@@ -35,14 +35,14 @@ pub struct Planner {
 
 #[derive(Copy, Clone, Debug)]
 pub struct Profile {
-    segments: [Segment; 7]
+    pub segments: [Segment; 8]
 }
 
 #[derive(Copy, Clone, Debug, Default)]
 pub struct Segment {
     // Used by executor
-    pub delta: [i32; 3],
-    start_time: u32, // 0 to disable; set after completion.
+    pub delta: [Wrapping<i32>; 3],
+    pub start_time: u32, // 0 to disable; set after completion.
     // used by planner
     v0: i32,
     a0: i32,
@@ -52,7 +52,7 @@ pub struct Segment {
 impl Segment {
     // 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 j = self.delta[2].0; // 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
@@ -87,15 +87,11 @@ 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) + (j / 6) + self.v0),
-                self.a0 + j,
-                j,
+                Wrapping((self.a0 / 2) + (j / 6) + self.v0),
+                Wrapping(self.a0 + j),
+                Wrapping(j),
             ],
             start_time: self.time,
             v0: self.v0,
@@ -104,7 +100,10 @@ impl State {
         }
     }
 
-    fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Segment {
+    fn produce_segment(&mut self, j: i32, length: u32, step_size: u32) -> Option<Segment> {
+        if length == 0 {
+            return None;
+        }
         let segment = self.segment_for(j);
         let t = length as i32;
         let t2 = t * length as i32;
@@ -119,7 +118,7 @@ impl State {
 
         self.time += length as u32;
 
-        segment
+        Some(segment)
     }
 }
 
@@ -173,7 +172,7 @@ impl Planner {
 
         // 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;
+        self.ta_max = (self.v_max / self.a_max).checked_sub(self.tj_max).unwrap_or(0);
 
         // Compute regime change points
         let a_max_2 = self.a_max as u64 * self.a_max as u64;
@@ -213,8 +212,9 @@ impl Planner {
         let tv = (dx - ramp_dx) * 2 / s4v as u64;
         let tv = ((tv + 1) / 2) as u32;
 
+        eprintln!("ta={ta}, tj={tj}, tv={tv}");
         let j_real = dir * 6;
-        let mut segments = [Segment::default(); 7];
+        let mut segments = [Segment::default(); 8];
         let seg_params = [
             (j_real, tj),
             (0, ta),
@@ -224,9 +224,19 @@ impl Planner {
             (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);
-        }
+
+        let step_size = self.step_size;
+
+
+        let mut imax = 0;
+        let seg_count =
+            seg_params.iter()
+                .copied()
+                .filter_map(|(j,t)| cstate.produce_segment(j, t,  step_size))
+                .enumerate()
+                .map(|(i, segment)| segments[i] = segment)
+                .count();
+        segments[seg_count].start_time = tj * 4 + ta * 2 + tv;
         Profile {
             segments,
         }