diff --git a/hostcontrol/Cargo.lock b/hostcontrol/Cargo.lock index 54a9403..dc422b4 100644 --- a/hostcontrol/Cargo.lock +++ b/hostcontrol/Cargo.lock @@ -1446,7 +1446,6 @@ dependencies = [ "eframe", "egui", "env_logger", - "log", "nalgebra", "nalgebra-macros", "serialport", @@ -1760,9 +1759,9 @@ dependencies = [ [[package]] name = "log" -version = "0.4.29" +version = "0.4.27" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897" +checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" [[package]] name = "mach2" diff --git a/hostcontrol/Cargo.toml b/hostcontrol/Cargo.toml index 16fa0d7..f83ff16 100644 --- a/hostcontrol/Cargo.toml +++ b/hostcontrol/Cargo.toml @@ -11,4 +11,3 @@ nalgebra-macros = "^0.2.2" env_logger = "0.11.8" egui = "0.31.1" eframe = "0.31.1" -log = "0.4.29" diff --git a/hostcontrol/src/main.rs b/hostcontrol/src/main.rs index fd7a634..4ccf95e 100644 --- a/hostcontrol/src/main.rs +++ b/hostcontrol/src/main.rs @@ -4,8 +4,7 @@ mod stage; fn main() { env_logger::init(); - let stage_io = stage::StageIO::new("/dev/ttyACM0", 115200).unwrap(); - let mut s = stage::Stage::new(stage_io, 1.12).unwrap(); + let mut s = stage::Stage::new("/dev/ttyACM0", 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 3d9fefb..f853e6a 100644 --- a/hostcontrol/src/stage.rs +++ b/hostcontrol/src/stage.rs @@ -2,7 +2,6 @@ use std::time::Duration; use thiserror::Error; -use log::debug; fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3 { nalgebra::matrix![ @@ -23,49 +22,46 @@ fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra:: ] } -#[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, + serial: Box, delta_pos: nalgebra::Vector3, cartesian_to_delta: nalgebra::Matrix3, delta_to_cartesian: nalgebra::Matrix3, } -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 }) +#[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) } - fn receive_until>(&mut self, needle: S) -> StageIOResult { + fn receive_until>(&mut self, needle: S) -> Result { 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| StageError::Read(e))?; res.push(buf[0]); if res.ends_with(needle.as_ref().as_bytes()) { return Ok(String::from_utf8_lossy(&res).to_string()); @@ -73,30 +69,11 @@ 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))?; - let response = self.receive_until("\n")?; - debug!("<-: {:?}", response); - Ok(response.trim().to_string()) + fn communicate>(&mut self, command: S) -> Result { + self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageError::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() @@ -108,7 +85,7 @@ impl Stage { } 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.communicate(format!("mr {} {} {}\n", d[0], d[1], d[2]))?; self.delta_pos += nalgebra::vector![d[0], d[1], d[2]]; Ok(()) } @@ -144,6 +121,6 @@ impl Stage { } pub fn version(&mut self) -> Result { - Ok(self.stage_io.send_request("version\n")?) + self.communicate("version\n") } }