diff --git a/hostcontrol/src/gcode_stage.rs b/hostcontrol/src/gcode_stage.rs new file mode 100644 index 0000000..28d5250 --- /dev/null +++ b/hostcontrol/src/gcode_stage.rs @@ -0,0 +1,45 @@ +use crate::stage_io::StageIO; +use crate::xy_stage::{Result, XYStage}; + +pub struct GCodeStage { + stage_io: StageIO, + max_range_μm: nalgebra::Vector3, +} + +impl GCodeStage { + pub fn new(stage_io: StageIO) -> Result { + 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) -> 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 + ))?; + 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 { + self.max_range_μm + } +} + +impl Drop for GCodeStage { + fn drop(&mut self) { + self.stage_io.send_request("M84\n").unwrap(); // switch off motors. + } +} diff --git a/hostcontrol/src/main.rs b/hostcontrol/src/main.rs index 7d525f5..4b1eb15 100644 --- a/hostcontrol/src/main.rs +++ b/hostcontrol/src/main.rs @@ -1,7 +1,14 @@ -use clap::Parser; +use clap::{Parser, ValueEnum}; use std::{thread::sleep, time::Duration}; -mod stage; +mod gcode_stage; +mod openflexure_stage; + +mod stage_io; +use crate::stage_io::StageIO; +use crate::xy_stage::XYStage; + +mod xy_stage; #[derive(clap::Parser, Debug)] #[command(version, about, long_about = None)] @@ -13,44 +20,43 @@ struct CliArgs { /// Speed of the stage device (bps) #[arg(long, default_value = "115200")] tty_speed: u32, + + #[arg(long, value_enum)] + backend: Backend, +} + +// [derive(DebugCopy, Clone, PartialEq, Eq, PartialOrd, Ord, ValueEnum)] +#[derive(Debug, Clone, ValueEnum)] +enum Backend { + /// OpenFlexure stage + OpenFlexure, + + /// GCode stage + GCode, } fn main() { let args = CliArgs::parse(); env_logger::init(); - let stage_io = stage::StageIO::new(&args.stage_device, args.tty_speed).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()); - - //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(); + let stage_io = StageIO::new(&args.stage_device, args.tty_speed).unwrap(); + let mut stage: Box = 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()) + } + }; + let origin = nalgebra::vector![0.0, 0.0, 0.0]; + stage.move_absolute_cartesian(origin).unwrap(); + sleep(Duration::from_secs(1)); + let mut max_xy = stage.get_range(); + max_xy[2] = 0.0; + stage.move_absolute_cartesian(max_xy).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(()) } +#[derive(Default)] 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| { @@ -58,25 +64,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()); -//} diff --git a/hostcontrol/src/stage.rs b/hostcontrol/src/openflexure_stage.rs similarity index 62% rename from hostcontrol/src/stage.rs rename to hostcontrol/src/openflexure_stage.rs index 8a4901b..2c98cda 100644 --- a/hostcontrol/src/stage.rs +++ b/hostcontrol/src/openflexure_stage.rs @@ -1,8 +1,7 @@ //! Stage controller for openflexure-delta like stage controllers. -use log::debug; -use std::time::Duration; -use thiserror::Error; +use crate::stage_io::StageIO; +use crate::xy_stage::{Result, StageError, XYStage}; fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3 { nalgebra::matrix![ @@ -23,74 +22,14 @@ 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 { +pub struct OpenFlexureStage { stage_io: StageIO, 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 }) - } - - fn receive_until>(&mut self, needle: S) -> StageIOResult { - let mut res: Vec = vec![]; - loop { - let mut buf = [0u8; 1]; - 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()); - } - } - } - - // 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()) - } -} - -pub type Result = std::result::Result; -impl Stage { +impl OpenFlexureStage { 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() @@ -142,7 +81,19 @@ impl Stage { self.move_relative_delta(diff) } - pub fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3) -> Result<()> { + pub fn move_relative_cartesian(&mut self, d: nalgebra::Vector3) -> Result<()> { + let abs = self.get_position_cartesian() + d; + self.move_absolute_cartesian(abs) + } + + pub fn version(&mut self) -> Result { + Ok(self.stage_io.send_request("version\n")?) + } +} + +impl XYStage for OpenFlexureStage { + fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3) -> 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]; @@ -162,12 +113,7 @@ impl Stage { self.move_absolute_delta(target) } - pub fn move_relative_cartesian(&mut self, d: nalgebra::Vector3) -> Result<()> { - let abs = self.get_position_cartesian() + d; - self.move_absolute_cartesian(abs) - } - - pub fn version(&mut self) -> Result { - Ok(self.stage_io.send_request("version\n")?) + fn get_range(&self) -> nalgebra::Vector3 { + nalgebra::vector![10000.0, 10000.0, 5000.0] } } diff --git a/hostcontrol/src/stage_io.rs b/hostcontrol/src/stage_io.rs new file mode 100644 index 0000000..1ffca22 --- /dev/null +++ b/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, +} + +pub type StageIOResult = std::result::Result; +impl StageIO { + /// Create a new serial StageIO at given serial device and speed. + 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 }) + } + + /// Receive data until the given character string "needle" is seen. + fn receive_until>(&mut self, needle: S) -> StageIOResult { + let mut res: Vec = 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>(&mut self, command: S) -> StageIOResult { + 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()) + } +} diff --git a/hostcontrol/src/xy_stage.rs b/hostcontrol/src/xy_stage.rs new file mode 100644 index 0000000..1f645c3 --- /dev/null +++ b/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 = std::result::Result; +pub trait XYStage { + /// Move to absolute coordinates (x,y,z) in micrometers. + fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3) -> 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; +}