diff --git a/hostcontrol/src/main.rs b/hostcontrol/src/main.rs index ce9d0c1..7d525f5 100644 --- a/hostcontrol/src/main.rs +++ b/hostcontrol/src/main.rs @@ -1,5 +1,5 @@ -use std::{thread::sleep, time::Duration}; use clap::Parser; +use std::{thread::sleep, time::Duration}; mod stage; @@ -7,11 +7,11 @@ mod stage; #[command(version, about, long_about = None)] struct CliArgs { /// Interface to talk to movement stage - #[arg(long, default_value="/dev/ttyACM0")] + #[arg(long, default_value = "/dev/ttyACM0")] stage_device: String, /// Speed of the stage device (bps) - #[arg(long, default_value="115200")] + #[arg(long, default_value = "115200")] tty_speed: u32, } @@ -43,8 +43,7 @@ fn main() { //Ok(()) } -struct App { -} +struct App {} impl Default for App { fn default() -> Self { diff --git a/hostcontrol/src/stage.rs b/hostcontrol/src/stage.rs index 3d9fefb..8a4901b 100644 --- a/hostcontrol/src/stage.rs +++ b/hostcontrol/src/stage.rs @@ -1,8 +1,8 @@ //! Stage controller for openflexure-delta like stage controllers. +use log::debug; use std::time::Duration; use thiserror::Error; -use log::debug; fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3 { nalgebra::matrix![ @@ -15,7 +15,7 @@ fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3 { fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra::Matrix3 { let x_fac = (2.0 / 3.0f64.sqrt()) * (flex_b / flex_h); let y_fac = -1.0 * flex_b / flex_h; - let z_fac = 1.0/3.0 * flex_b / flex_a; + let z_fac = 1.0 / 3.0 * flex_b / flex_a; nalgebra::matrix![ -x_fac, x_fac, 0.0; 0.5 * y_fac, 0.5 * y_fac, -y_fac; @@ -55,17 +55,21 @@ pub struct Stage { pub type StageIOResult = std::result::Result; impl StageIO { pub fn new(port: &str, speed: u32) -> StageIOResult { - let serial = serialport::new(port, speed).timeout(Duration::from_secs(60)).open()?; - // TODO: read and discard initial characters; some devices like to be - // chatty on start-up. - Ok(Self { serial }) + let serial = serialport::new(port, speed) + .timeout(Duration::from_secs(60)) + .open()?; + // TODO: read and discard initial characters; some devices like to be + // chatty on start-up. + Ok(Self { serial }) } fn receive_until>(&mut self, needle: S) -> StageIOResult { let mut res: Vec = vec![]; loop { let mut buf = [0u8; 1]; - self.serial.read_exact(&mut buf).map_err(|e| StageIOError::Read(e))?; + self.serial + .read_exact(&mut buf) + .map_err(|e| StageIOError::Read(e))?; res.push(buf[0]); if res.ends_with(needle.as_ref().as_bytes()) { return Ok(String::from_utf8_lossy(&res).to_string()); @@ -75,10 +79,12 @@ impl StageIO { // Send request and wait for response deliminated with a newline pub fn send_request>(&mut self, command: S) -> StageIOResult { - debug!("->: {:?}", command.as_ref()); - self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageIOError::Write(e))?; + debug!("->: {:?}", command.as_ref()); + self.serial + .write_all(command.as_ref().as_bytes()) + .map_err(|e| StageIOError::Write(e))?; let response = self.receive_until("\n")?; - debug!("<-: {:?}", response); + debug!("<-: {:?}", response); Ok(response.trim().to_string()) } } @@ -87,28 +93,46 @@ pub type Result = std::result::Result; impl Stage { pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result { // TODO: non-blocking moves - let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() * new_delta_into_cartesian(80.0, 50.0, 50.0); + let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() + * new_delta_into_cartesian(80.0, 50.0, 50.0); let cartesian_to_delta = delta_to_cartesian.try_inverse().unwrap(); - let mut res = Self { stage_io, delta_pos: nalgebra::vector![0, 0, 0], cartesian_to_delta, delta_to_cartesian }; + let mut res = Self { + stage_io, + delta_pos: nalgebra::vector![0, 0, 0], + cartesian_to_delta, + delta_to_cartesian, + }; - let pos = res.stage_io.send_request("p?")?.split(" ").map(|v| v.parse::().map_err(|_| StageError::Mumble)).collect::, _>>()?; + let pos = res + .stage_io + .send_request("p?")? + .split(" ") + .map(|v| v.parse::().map_err(|_| StageError::Mumble)) + .collect::, _>>()?; res.delta_pos = nalgebra::vector![pos[0], pos[1], pos[2]]; - println!("Initialized stage at delta positions {}/{}/{}", pos[0], pos[1], pos[2]); + println!( + "Initialized stage at delta positions {}/{}/{}", + pos[0], pos[1], pos[2] + ); Ok(res) } - pub fn get_position_delta(&self) -> nalgebra::Vector3 { self.delta_pos.clone() } pub fn get_position_cartesian(&self) -> nalgebra::Vector3 { - let pos = nalgebra::vector![self.delta_pos[0] as f64, self.delta_pos[1] as f64, self.delta_pos[2] as f64]; + let pos = nalgebra::vector![ + self.delta_pos[0] as f64, + self.delta_pos[1] as f64, + self.delta_pos[2] as f64 + ]; self.delta_to_cartesian * pos } pub fn move_relative_delta(&mut self, d: nalgebra::Vector3) -> Result<()> { - self.stage_io.send_request(format!("mr {} {} {}\n", d[0], d[1], d[2]))?; + self.stage_io + .send_request(format!("mr {} {} {}\n", d[0], d[1], d[2]))?; self.delta_pos += nalgebra::vector![d[0], d[1], d[2]]; Ok(()) }