thequux/film-scanner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added motion test tool

Browse Source
This commit is contained in:
TQ Hirsch
2022-04-15 22:47:14 +02:00
parent 9c23fcbabb
commit 89358a6aab
7 changed files with 321 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
docs/motion-control.ipynb
View File

@@ -649,7 +649,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.9.10"
"version": "3.9.11"
}
},
"nbformat": 4,

223
firmware/Cargo.lock generated
View File

@@ -2,6 +2,229 @@
# It is not intended for manual editing.
version = 3
[[package]]
name = "ansi_term"
version = "0.12.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "d52a9bb7ec0cf484c551830a7ce27bd20d67eac647e1befb56b0be4ee39a55d2"
dependencies = [
"winapi",
]
[[package]]
name = "atty"
version = "0.2.14"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8"
dependencies = [
"hermit-abi",
"libc",
"winapi",
]
[[package]]
name = "bitflags"
version = "1.3.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
[[package]]
name = "clap"
version = "2.34.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a0610544180c38b88101fecf2dd634b174a62eef6946f84dfc6a7127512b381c"
dependencies = [
"ansi_term",
"atty",
"bitflags",
"strsim",
"textwrap",
"unicode-width",
"vec_map",
]
[[package]]
name = "heck"
version = "0.3.3"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "6d621efb26863f0e9924c6ac577e8275e5e6b77455db64ffa6c65c904e9e132c"
dependencies = [
"unicode-segmentation",
]
[[package]]
name = "hermit-abi"
version = "0.1.19"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33"
dependencies = [
"libc",
]
[[package]]
name = "jerk_control"
version = "0.1.0"
[[package]]
name = "lazy_static"
version = "1.4.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
[[package]]
name = "libc"
version = "0.2.123"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cb691a747a7ab48abc15c5b42066eaafde10dc427e3b6ee2a1cf43db04c763bd"
[[package]]
name = "motion-tester"
version = "0.1.0"
dependencies = [
"jerk_control",
"structopt",
]
[[package]]
name = "proc-macro-error"
version = "1.0.4"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c"
dependencies = [
"proc-macro-error-attr",
"proc-macro2",
"quote",
"syn",
"version_check",
]
[[package]]
name = "proc-macro-error-attr"
version = "1.0.4"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869"
dependencies = [
"proc-macro2",
"quote",
"version_check",
]
[[package]]
name = "proc-macro2"
version = "1.0.37"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "ec757218438d5fda206afc041538b2f6d889286160d649a86a24d37e1235afd1"
dependencies = [
"unicode-xid",
]
[[package]]
name = "quote"
version = "1.0.18"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a1feb54ed693b93a84e14094943b84b7c4eae204c512b7ccb95ab0c66d278ad1"
dependencies = [
"proc-macro2",
]
[[package]]
name = "strsim"
version = "0.8.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8ea5119cdb4c55b55d432abb513a0429384878c15dde60cc77b1c99de1a95a6a"
[[package]]
name = "structopt"
version = "0.3.26"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "0c6b5c64445ba8094a6ab0c3cd2ad323e07171012d9c98b0b15651daf1787a10"
dependencies = [
"clap",
"lazy_static",
"structopt-derive",
]
[[package]]
name = "structopt-derive"
version = "0.4.18"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "dcb5ae327f9cc13b68763b5749770cb9e048a99bd9dfdfa58d0cf05d5f64afe0"
dependencies = [
"heck",
"proc-macro-error",
"proc-macro2",
"quote",
"syn",
]
[[package]]
name = "syn"
version = "1.0.91"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b683b2b825c8eef438b77c36a06dc262294da3d5a5813fac20da149241dcd44d"
dependencies = [
"proc-macro2",
"quote",
"unicode-xid",
]
[[package]]
name = "textwrap"
version = "0.11.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "d326610f408c7a4eb6f51c37c330e496b08506c9457c9d34287ecc38809fb060"
dependencies = [
"unicode-width",
]
[[package]]
name = "unicode-segmentation"
version = "1.9.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7e8820f5d777f6224dc4be3632222971ac30164d4a258d595640799554ebfd99"
[[package]]
name = "unicode-width"
version = "0.1.9"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3ed742d4ea2bd1176e236172c8429aaf54486e7ac098db29ffe6529e0ce50973"
[[package]]
name = "unicode-xid"
version = "0.2.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8ccb82d61f80a663efe1f787a51b16b5a51e3314d6ac365b08639f52387b33f3"
[[package]]
name = "vec_map"
version = "0.8.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191"
[[package]]
name = "version_check"
version = "0.9.4"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f"
[[package]]
name = "winapi"
version = "0.3.9"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419"
dependencies = [
"winapi-i686-pc-windows-gnu",
"winapi-x86_64-pc-windows-gnu",
]
[[package]]
name = "winapi-i686-pc-windows-gnu"
version = "0.4.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6"
[[package]]
name = "winapi-x86_64-pc-windows-gnu"
version = "0.4.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"

2
firmware/Cargo.toml
View File

@@ -1,2 +1,2 @@
[workspace]
members = [ "jerk_control" ]
members = [ "jerk_control", "motion-tester" ]

66
firmware/jerk_control/src/executor.rs Normal file
View File

@@ -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
}
}

2
firmware/jerk_control/src/lib.rs Normal file
View File

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

21
firmware/jerk_control/src/main.rs
View File

@@ -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);
}

46
firmware/jerk_control/src/motion/planner.rs → firmware/jerk_control/src/planner.rs
View File

@@ -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,
}