init from q3k's code dump

hostcontrol/src/stage.rs

@@ -0,0 +1,126 @@
+//! Stage controller for openflexure-delta like stage controllers.
+
+use std::time::Duration;
+use thiserror::Error;
+
+fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
+    nalgebra::matrix![
+        theta.cos(), -theta.sin(), 0.0;
+        theta.sin(), theta.cos(), 0.0;
+        0.0, 0.0, 1.0
+    ]
+}
+
+fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra::Matrix3<f64> {
+    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;
+    nalgebra::matrix![
+        -x_fac, x_fac, 0.0;
+        0.5 * y_fac, 0.5 * y_fac, -y_fac;
+        z_fac, z_fac, z_fac
+    ]
+}
+
+pub struct Stage {
+    serial: Box<dyn serialport::SerialPort>,
+    delta_pos: nalgebra::Vector3<i32>,
+    cartesian_to_delta: nalgebra::Matrix3<f64>,
+    delta_to_cartesian: nalgebra::Matrix3<f64>,
+}
+
+#[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<T> = std::result::Result<T, StageError>;
+
+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))?;
+            res.push(buf[0]);
+            if res.ends_with(needle.as_ref().as_bytes()) {
+                return Ok(String::from_utf8_lossy(&res).to_string());
+            }
+        }
+    }
+
+    fn communicate<S: AsRef<str>>(&mut self, command: S) -> Result<String> {
+        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 fn get_position_delta(&self) -> nalgebra::Vector3<i32> {
+        self.delta_pos.clone()
+    }
+
+    pub fn get_position_cartesian(&self) -> nalgebra::Vector3<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<i32>) -> Result<()> {
+        self.communicate(format!("mr {} {} {}\n", d[0], d[1], d[2]))?;
+        self.delta_pos += nalgebra::vector![d[0], d[1], d[2]];
+        Ok(())
+    }
+
+    pub fn move_absolute_delta(&mut self, a: nalgebra::Vector3<i32>) -> Result<()> {
+        let diff = a - self.delta_pos;
+        self.move_relative_delta(diff)
+    }
+
+    pub fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3<f64>) -> Result<()> {
+        let current = self.get_position_delta();
+        let target = self.cartesian_to_delta * a;
+        let target = nalgebra::vector![target[0] as i32, target[1] as i32, target[2] as i32];
+
+        // Calculate backlash compensation - always approach from the same
+        // direction by at least 500 units.
+        let mut backlash = target.clone();
+        for i in [0, 1, 2] {
+            if current[i] > target[i] {
+                backlash[i] -= 500;
+            }
+        }
+        if target != backlash {
+            // Backlash compensation move before main move, if needed.
+            self.move_absolute_delta(backlash)?;
+        }
+        self.move_absolute_delta(target)
+    }
+
+    pub fn move_relative_cartesian(&mut self, d: nalgebra::Vector3<f64>) -> Result<()> {
+        let abs = self.get_position_cartesian() + d;
+        self.move_absolute_cartesian(abs)
+    }
+
+    pub fn version(&mut self) -> Result<String> {
+        self.communicate("version\n")
+    }
+}