Provide some debug output.

Signed-off-by: Henner Zeller <h.zeller@acm.org>

hostcontrol/Cargo.lock

@@ -1446,6 +1446,7 @@ dependencies = [
  "eframe",
  "egui",
  "env_logger",
+ "log",
  "nalgebra",
  "nalgebra-macros",
  "serialport",
@@ -1759,9 +1760,9 @@ dependencies = [
 
 [[package]]
 name = "log"
-version = "0.4.27"
+version = "0.4.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
+checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897"
 
 [[package]]
 name = "mach2"

hostcontrol/Cargo.toml

@@ -11,3 +11,4 @@ nalgebra-macros = "^0.2.2"
 env_logger = "0.11.8"
 egui = "0.31.1"
 eframe = "0.31.1"
+log = "0.4.29"

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());
 

hostcontrol/src/stage.rs

@@ -2,6 +2,7 @@
 
 use std::time::Duration;
 use thiserror::Error;
+use log::debug;
 
 fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
     nalgebra::matrix![
@@ -22,46 +23,49 @@ 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<dyn serialport::SerialPort>,
+}
+
+pub struct Stage {
+    stage_io: StageIO,
     delta_pos: nalgebra::Vector3<i32>,
     cartesian_to_delta: nalgebra::Matrix3<f64>,
     delta_to_cartesian: nalgebra::Matrix3<f64>,
 }
 
-#[derive(Error, Debug)]
+pub type StageIOResult<T> = std::result::Result<T, StageIOError>;
-pub enum StageError {
+impl StageIO {
-    #[error("could not connect to stage controller")]
+    pub fn new(port: &str, speed: u32) -> StageIOResult<Self> {
-    Open(serialport::Error),
+	let serial = serialport::new(port, speed).timeout(Duration::from_secs(60)).open()?;
-    #[error("error when sending data to the stage controller")]
+	// TODO: read and discard initial characters; some devices like to be
-    Write(std::io::Error),
+	// chatty on start-up.
-    #[error("error when receiving data from the stage controller")]
+	Ok(Self { serial })
-    Read(std::io::Error),
-    #[error("protocol error")]
-    Mumble,
     }
 
-pub type Result<T> = std::result::Result<T, StageError>;
+    fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> StageIOResult<String> {
-
-impl Stage {
-    pub fn new(port: &str, camera_rotation: f64) -> Result<Self> {
-        // 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::<i32>().map_err(|_| StageError::Mumble)).collect::<std::result::Result<Vec<_>, _>>()?;
-        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<S: AsRef<str>>(&mut self, needle: S) -> Result<String> {
         let mut res: Vec<u8> = 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 +73,30 @@ impl Stage {
         }
     }
 
-    fn communicate<S: AsRef<str>>(&mut self, command: S) -> Result<String> {
+    // Send request and wait for response deliminated with a newline
-        self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageError::Write(e))?;
+    pub fn send_request<S: AsRef<str>>(&mut self, command: S) -> StageIOResult<String> {
-        let res = self.receive_until("\n")?;
+	debug!("->: {:?}", command.as_ref());
-        Ok(res.trim().to_string())
+        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())
     }
+}
+
+pub type Result<T> = std::result::Result<T, StageError>;
+impl Stage {
+    pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result<Self> {
+        // 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::<i32>().map_err(|_| StageError::Mumble)).collect::<std::result::Result<Vec<_>, _>>()?;
+        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<i32> {
         self.delta_pos.clone()
@@ -85,7 +108,7 @@ impl Stage {
     }
 
     pub fn move_relative_delta(&mut self, d: nalgebra::Vector3<i32>) -> 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 +144,6 @@ impl Stage {
     }
 
     pub fn version(&mut self) -> Result<String> {
-        self.communicate("version\n")
+        Ok(self.stage_io.send_request("version\n")?)
     }
 }