Properly report the position in filename.

There might be better camera capture crates, this
is just the first pick for now.

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

hostcontrol/.gitignore

@@ -1 +1,5 @@
 /target
+.envrc
+.direnv/
+*~
+

hostcontrol/Cargo.toml

@@ -4,6 +4,8 @@ version = "0.1.0"
 edition = "2021"
 
 [dependencies]
+anyhow = "1.0.100"
+clap = { version = "4.0", features = ["derive"] }
 nalgebra = { version = "0.33" }
 serialport = { version = "4", default-features = false }
 thiserror = "2"
@@ -11,3 +13,6 @@ nalgebra-macros = "^0.2.2"
 env_logger = "0.11.8"
 egui = "0.31.1"
 eframe = "0.31.1"
+log = "0.4.29"
+image = { version = "0.25", default-features = false, features = ["png","jpeg"] }
+nokhwa = { version = "0.10.0", features = [ "input-native" ] }

hostcontrol/shell.nix

@@ -5,6 +5,9 @@ pkgs.mkShell {
   buildInputs = with pkgs; [
     cargo
 
+    # Needed for a bindgen dependency in nokhwa
+    rustPlatform.bindgenHook
+
     # Useful tools
     clippy
     rustfmt

hostcontrol/src/camera.rs

@@ -0,0 +1,31 @@
+use anyhow::{Context, Result};
+use image::RgbImage;
+use nokhwa::pixel_format::RgbFormat;
+use nokhwa::utils::{CameraIndex, RequestedFormat, RequestedFormatType};
+use nokhwa::Camera as NokhwaCamera;
+
+pub struct Camera {
+    camera: NokhwaCamera,
+}
+impl Camera {
+    pub fn new(camera_index: u32) -> Result<Self> {
+        let index = CameraIndex::Index(camera_index);
+        let format =
+            RequestedFormat::new::<RgbFormat>(RequestedFormatType::AbsoluteHighestResolution);
+        let mut camera =
+            NokhwaCamera::new(index, format).context("could not find/access webcam")?;
+        camera.open_stream().context("failed to open stream")?;
+
+        Ok(Self { camera })
+    }
+
+    pub fn capture(&mut self) -> Result<RgbImage> {
+        for _ in 0..5 {
+            // Hack to have camera flush frames in buffer.
+            let _ = self.camera.frame();
+        }
+        let frame = self.camera.frame().context("Could not capture image")?;
+        println!("snap!");
+        Ok(frame.decode_image::<RgbFormat>()?)
+    }
+}

hostcontrol/src/gcode_stage.rs

@@ -0,0 +1,46 @@
+use crate::stage_io::StageIO;
+use crate::xy_stage::{Result, XYStage};
+
+pub struct GCodeStage {
+    stage_io: StageIO,
+    max_range_μm: nalgebra::Vector3<f64>,
+}
+
+impl GCodeStage {
+    pub fn new(stage_io: StageIO) -> Result<Self> {
+        let mut res = Self {
+            stage_io,
+            // TODO: determine max range with some M-code ?
+            max_range_μm: nalgebra::vector![100.0 * 1000.0, 100.0 * 1000.0, 50.0 * 1000.0],
+        };
+        res.stage_io.send_request("G28\n")?;
+        Ok(res)
+    }
+}
+
+impl XYStage for GCodeStage {
+    /// Move to absolute coordinates (x,y,z) in micrometers.
+    fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3<f64>) -> Result<()> {
+        // TODO: check range
+        self.stage_io.send_request(format!(
+            "G1 X{} Y{} Z{}\n",
+            a[0] / 1000.0,
+            a[1] / 1000.0,
+            a[2] / 1000.0
+        ))?;
+        self.stage_io.send_request("G4 P1\n")?; // make sure move finished.
+        Ok(())
+    }
+
+    /// Get range of valid cartesian coordinates box. For now, assuming one
+    /// corner is fixed at (0,0,0), the other remote corner is this range.
+    fn get_range(&self) -> nalgebra::Vector3<f64> {
+        self.max_range_μm
+    }
+}
+
+impl Drop for GCodeStage {
+    fn drop(&mut self) {
+        self.stage_io.send_request("M84\n").unwrap(); // switch off motors.
+    }
+}

hostcontrol/src/main.rs

@@ -1,41 +1,96 @@
-use std::{thread::sleep, time::Duration};
+use clap::{Parser, ValueEnum};
+use image::RgbImage;
+use std::path::PathBuf;
 
-mod stage;
+mod camera;
+mod gcode_stage;
+mod openflexure_stage;
+
+mod stage_io;
+use crate::camera::Camera;
+use crate::stage_io::StageIO;
+use crate::xy_stage::XYStage;
+
+mod xy_stage;
+
+#[derive(clap::Parser, Debug)]
+#[command(version, about, long_about = None)]
+struct CliArgs {
+    /// Interface to talk to movement stage
+    #[arg(long, default_value = "/dev/ttyACM0")]
+    stage_device: String,
+
+    /// Speed of the stage device (bps)
+    #[arg(long, default_value = "115200")]
+    tty_speed: u32,
+
+    /// Stage movement backend
+    #[arg(long, value_enum)]
+    backend: Backend,
+
+    /// Id of camera to fetch images from.
+    #[arg(long, default_value = "0")]
+    camera_index: u32,
+
+    /// Directory all captured images are stored.
+    #[arg(long, value_name = "out-dir")]
+    output_directory: PathBuf,
+}
+
+// [derive(DebugCopy, Clone, PartialEq, Eq, PartialOrd, Ord, ValueEnum)]
+#[derive(Debug, Clone, ValueEnum)]
+enum Backend {
+    /// OpenFlexure stage
+    OpenFlexure,
+
+    /// GCode stage
+    GCode,
+}
+
+fn store_image(number: u32, pos: nalgebra::Vector3<f64>, dir: &PathBuf, img: &RgbImage) {
+    let img_file = &dir.join(format!("img-{:05}-{:0},{:0}.png", number, pos[0], pos[1]));
+
+    img.save(img_file).unwrap();
+}
 
 fn main() {
     env_logger::init();
-    let mut s = stage::Stage::new("/dev/ttyACM0", 1.12).unwrap();
+    let args = CliArgs::parse();
-    let origin = nalgebra::vector![3000.0, 0.0, -6000.0];
-    //println!("{}", s.version().unwrap());
 
-    //let a = nalgebra::vector![-10000.0, 0.0, -1250.0];
+    if !args.output_directory.is_dir() {
-    //let b = nalgebra::vector![18000.0, -15000.0, -1600.0];
+        panic!("--out-dir needs to be an existing directory");
-    //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 {
+    let origin = nalgebra::vector![0.0, 0.0, 0.0];
+
+    // Picture source
+    let mut cam = Camera::new(args.camera_index).unwrap();
+    cam.capture().unwrap(); // Warm up camera.
+
+    // Movement
+    let stage_io = StageIO::new(&args.stage_device, args.tty_speed).unwrap();
+    let mut stage: Box<dyn XYStage> = match args.backend {
+        Backend::GCode => Box::new(gcode_stage::GCodeStage::new(stage_io).unwrap()),
+        Backend::OpenFlexure => {
+            Box::new(openflexure_stage::OpenFlexureStage::new(stage_io, 1.12).unwrap())
+        }
+    };
+
+    // Local testing. Just take a line of pictures.
+    let mut max_xy = stage.get_range();
+    // Only interested in lerp-ing X for now.
+    max_xy[1] = 0.0;
+    max_xy[2] = 0.0;
+
+    for n in 0..10 {
+        let pos = max_xy * n as f64 / 10.0;
+        stage.move_absolute_cartesian(pos).unwrap();
+        store_image(n, pos, &args.output_directory, &cam.capture().unwrap());
+    }
 }
 
-impl Default for App {
+#[derive(Default)]
-    fn default() -> Self {
+struct App {}
-        App {}
-    }
-}
 
 impl eframe::App for App {
     fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) {
@@ -44,25 +99,3 @@ impl eframe::App for App {
         });
     }
 }
-
-//
-//    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/openflexure_stage.rs

@@ -1,7 +1,7 @@
 //! Stage controller for openflexure-delta like stage controllers.
 
-use std::time::Duration;
+use crate::stage_io::StageIO;
-use thiserror::Error;
+use crate::xy_stage::{Result, StageError, XYStage};
 
 fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
     nalgebra::matrix![
@@ -22,70 +22,56 @@ fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra::
     ]
 }
 
-pub struct Stage {
+pub struct OpenFlexureStage {
-    serial: Box<dyn serialport::SerialPort>,
+    stage_io: StageIO,
     delta_pos: nalgebra::Vector3<i32>,
     cartesian_to_delta: nalgebra::Matrix3<f64>,
     delta_to_cartesian: nalgebra::Matrix3<f64>,
 }
 
-#[derive(Error, Debug)]
+impl OpenFlexureStage {
-pub enum StageError {
+    pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result<Self> {
-    #[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()
-        let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() * new_delta_into_cartesian(80.0, 50.0, 50.0);
+            * 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 mut res = Self {
+            stage_io,
+            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<_>, _>>()?;
+        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]);
+        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];
+        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.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(())
     }
@@ -95,7 +81,19 @@ impl Stage {
         self.move_relative_delta(diff)
     }
 
-    pub fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3<f64>) -> Result<()> {
+    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> {
+        Ok(self.stage_io.send_request("version\n")?)
+    }
+}
+
+impl XYStage for OpenFlexureStage {
+    fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3<f64>) -> Result<()> {
+        // TODO: check range with get_movement_box()
         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];
@@ -115,12 +113,7 @@ impl Stage {
         self.move_absolute_delta(target)
     }
 
-    pub fn move_relative_cartesian(&mut self, d: nalgebra::Vector3<f64>) -> Result<()> {
+    fn get_range(&self) -> nalgebra::Vector3<f64> {
-        let abs = self.get_position_cartesian() + d;
+        nalgebra::vector![10000.0, 10000.0, 5000.0]
-        self.move_absolute_cartesian(abs)
-    }
-
-    pub fn version(&mut self) -> Result<String> {
-        self.communicate("version\n")
     }
 }

hostcontrol/src/stage_io.rs

@@ -0,0 +1,56 @@
+use log::debug;
+use std::time::Duration;
+use thiserror::Error;
+
+#[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),
+}
+
+pub struct StageIO {
+    serial: Box<dyn serialport::SerialPort>,
+}
+
+pub type StageIOResult<T> = std::result::Result<T, StageIOError>;
+impl StageIO {
+    /// Create a new serial StageIO at given serial device and speed.
+    pub fn new(port: &str, speed: u32) -> StageIOResult<Self> {
+        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 })
+    }
+
+    /// Receive data until the given character string "needle" is seen.
+    fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> StageIOResult<String> {
+        let mut res: Vec<u8> = vec![];
+        loop {
+            let mut buf = [0u8; 1];
+            self.serial
+                .read_exact(&mut buf)
+                .map_err(StageIOError::Read)?;
+            res.push(buf[0]);
+            if res.ends_with(needle.as_ref().as_bytes()) {
+                return Ok(String::from_utf8_lossy(&res).to_string());
+            }
+        }
+    }
+
+    /// Send request and wait for response deliminated with a newline
+    pub fn send_request<S: AsRef<str>>(&mut self, command: S) -> StageIOResult<String> {
+        debug!("->: {:?}", command.as_ref());
+        self.serial
+            .write_all(command.as_ref().as_bytes())
+            .map_err(StageIOError::Write)?;
+        let response = self.receive_until("\n")?;
+        debug!("<-: {:?}", response);
+        Ok(response.trim().to_string())
+    }
+}

hostcontrol/src/xy_stage.rs

@@ -0,0 +1,20 @@
+use crate::stage_io::StageIOError;
+use thiserror::Error;
+
+#[derive(Error, Debug)]
+pub enum StageError {
+    #[error("Interface issue")]
+    Communication(#[from] StageIOError),
+    #[error("protocol error")]
+    Mumble,
+}
+
+pub type Result<T> = std::result::Result<T, StageError>;
+pub trait XYStage {
+    /// Move to absolute coordinates (x,y,z) in micrometers.
+    fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3<f64>) -> Result<()>;
+
+    /// Get range of valid cartesian coordinates box. For now, assuming one
+    /// corner is fixed at (0,0,0), the other remote corner is this range.
+    fn get_range(&self) -> nalgebra::Vector3<f64>;
+}