init from q3k's code dump

hostcontrol/.gitignore

@@ -0,0 +1 @@
+/target

hostcontrol/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "hostcontrol"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+nalgebra = { version = "0.33" }
+serialport = { version = "4", default-features = false }
+thiserror = "2"
+nalgebra-macros = "^0.2.2"
+env_logger = "0.11.8"
+egui = "0.31.1"
+eframe = "0.31.1"

hostcontrol/shell.nix

@@ -0,0 +1,7 @@
+with import <nixpkgs> {};
+
+pkgs.mkShell {
+  LD_LIBRARY_PATH = [
+    "${xorg.libX11}/lib;${xorg.libXcursor}/lib;${xorg.libXi}/lib;${libxkbcommon}/lib"
+  ];
+}

hostcontrol/src/main.rs

@@ -0,0 +1,68 @@
+use std::{thread::sleep, time::Duration};
+
+mod stage;
+
+fn main() {
+    env_logger::init();
+    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());
+
+    //let a = nalgebra::vector![-10000.0, 0.0, -1250.0];
+    //let b = nalgebra::vector![18000.0, -15000.0, -1600.0];
+    //let c = nalgebra::vector![0.0, -15000.0, -250.0];
+
+    s.move_absolute_cartesian(origin).unwrap();
+    sleep(Duration::from_secs(5));
+    //s.move_absolute_cartesian(origin + a).unwrap();
+    ////sleep(Duration::from_secs(5));
+    ////s.move_absolute_cartesian(origin).unwrap();
+    ////s.move_absolute_cartesian(origin + c).unwrap();
+    ////sleep(Duration::from_secs(5));
+    ////s.move_absolute_cartesian(origin + nalgebra::vector![0.0, -16000.0, -500.0]).unwrap();
+    ////println!("D");
+    ////sleep(Duration::from_secs(5));
+    ////s.move_absolute_cartesian(origin + nalgebra::vector![0.0, 0.0, 0.0]).unwrap();
+    //println!("{:?}", s.get_position_cartesian());
+
+    //Ok(())
+}
+
+struct App {
+}
+
+impl Default for App {
+    fn default() -> Self {
+        App {}
+    }
+}
+
+impl eframe::App for App {
+    fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) {
+        egui::CentralPanel::default().show(ctx, |ui| {
+            ui.heading("My egui Application");
+        });
+    }
+}
+
+//
+//    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());
+//
+//    let a = nalgebra::vector![18000.0, 0.0, -1250.0];
+//    let b = nalgebra::vector![18000.0, -15000.0, -1600.0];
+//    let c = nalgebra::vector![0.0, -15000.0, -250.0];
+//
+//    s.move_absolute_cartesian(origin).unwrap();
+//    //sleep(Duration::from_secs(5));
+//    //s.move_absolute_cartesian(origin + a).unwrap();
+//    //sleep(Duration::from_secs(5));
+//    //s.move_absolute_cartesian(origin + c).unwrap();
+//    //sleep(Duration::from_secs(5));
+//    //s.move_absolute_cartesian(origin + nalgebra::vector![0.0, -16000.0, -500.0]).unwrap();
+//    //println!("D");
+//    //sleep(Duration::from_secs(5));
+//    //s.move_absolute_cartesian(origin + nalgebra::vector![0.0, 0.0, 0.0]).unwrap();
+//    println!("{:?}", s.get_position_cartesian());
+//}

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")
+    }
+}