From 39147d45f45ea660e1ce44593a02b22a85e0b0da Mon Sep 17 00:00:00 2001 From: Henner Zeller Date: Sun, 22 Feb 2026 11:49:17 +0100 Subject: [PATCH] Break out serial interface to be used in other stages. Signed-off-by: Henner Zeller --- hostcontrol/src/main.rs | 3 +- hostcontrol/src/stage.rs | 84 ++++++++++++++++++++++++---------------- 2 files changed, 53 insertions(+), 34 deletions(-) diff --git a/hostcontrol/src/main.rs b/hostcontrol/src/main.rs index 4ccf95e..fd7a634 100644 --- a/hostcontrol/src/main.rs +++ b/hostcontrol/src/main.rs @@ -4,7 +4,8 @@ mod stage; fn main() { env_logger::init(); - let mut s = stage::Stage::new("/dev/ttyACM0", 1.12).unwrap(); + let stage_io = stage::StageIO::new("/dev/ttyACM0", 115200).unwrap(); + let mut s = stage::Stage::new(stage_io, 1.12).unwrap(); let origin = nalgebra::vector![3000.0, 0.0, -6000.0]; //println!("{}", s.version().unwrap()); diff --git a/hostcontrol/src/stage.rs b/hostcontrol/src/stage.rs index f853e6a..2f510b5 100644 --- a/hostcontrol/src/stage.rs +++ b/hostcontrol/src/stage.rs @@ -22,46 +22,47 @@ fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra:: ] } -pub struct Stage { +#[derive(Error, Debug)] +pub enum StageIOError { + #[error("could not connect to stage controller")] + Open(#[from] serialport::Error), + #[error("error when sending data to the stage controller")] + Write(std::io::Error), + #[error("error when receiving data from the stage controller")] + Read(std::io::Error), +} + +#[derive(Error, Debug)] +pub enum StageError { + #[error("Interface issue")] + Communication(#[from] StageIOError), + #[error("protocol error")] + Mumble, +} + +pub struct StageIO { serial: Box, +} + +pub struct Stage { + stage_io: StageIO, delta_pos: nalgebra::Vector3, cartesian_to_delta: nalgebra::Matrix3, delta_to_cartesian: nalgebra::Matrix3, } -#[derive(Error, Debug)] -pub enum StageError { - #[error("could not connect to stage controller")] - Open(serialport::Error), - #[error("error when sending data to the stage controller")] - Write(std::io::Error), - #[error("error when receiving data from the stage controller")] - Read(std::io::Error), - #[error("protocol error")] - Mumble, -} - -pub type Result = std::result::Result; - -impl Stage { - pub fn new(port: &str, camera_rotation: f64) -> Result { - // TODO: non-blocking moves - let serial = serialport::new(port, 115200).timeout(Duration::from_secs(60)).open().map_err(|e| StageError::Open(e))?; - 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 { serial, delta_pos: nalgebra::vector![0, 0, 0], cartesian_to_delta, delta_to_cartesian }; - - let pos = res.communicate("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]); - Ok(res) +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()?; + Ok(Self { serial }) } - fn receive_until>(&mut self, needle: S) -> Result { + 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| StageError::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()); @@ -69,11 +70,28 @@ impl Stage { } } - fn communicate>(&mut self, command: S) -> Result { - self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageError::Write(e))?; + // Send request and wait for response deliminated with a newline + fn send_request>(&mut self, command: S) -> StageIOResult { + self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageIOError::Write(e))?; let res = self.receive_until("\n")?; Ok(res.trim().to_string()) } +} + +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 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 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]); + Ok(res) + } + pub fn get_position_delta(&self) -> nalgebra::Vector3 { self.delta_pos.clone() @@ -85,7 +103,7 @@ impl Stage { } pub fn move_relative_delta(&mut self, d: nalgebra::Vector3) -> Result<()> { - self.communicate(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(()) } @@ -121,6 +139,6 @@ impl Stage { } pub fn version(&mut self) -> Result { - self.communicate("version\n") + Ok(self.stage_io.send_request("version\n")?) } }