diff --git a/hostcontrol/src/gcode_stage.rs b/hostcontrol/src/gcode_stage.rs deleted file mode 100644 index 28d5250..0000000 --- a/hostcontrol/src/gcode_stage.rs +++ /dev/null @@ -1,45 +0,0 @@ -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 4b1eb15..7d525f5 100644 --- a/hostcontrol/src/main.rs +++ b/hostcontrol/src/main.rs @@ -1,14 +1,7 @@ -use clap::{Parser, ValueEnum}; +use clap::Parser; use std::{thread::sleep, time::Duration}; -mod gcode_stage; -mod openflexure_stage; - -mod stage_io; -use crate::stage_io::StageIO; -use crate::xy_stage::XYStage; - -mod xy_stage; +mod stage; #[derive(clap::Parser, Debug)] #[command(version, about, long_about = None)] @@ -20,43 +13,44 @@ 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 = 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(); + 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(); 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| { @@ -64,3 +58,25 @@ 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/openflexure_stage.rs b/hostcontrol/src/stage.rs similarity index 62% rename from hostcontrol/src/openflexure_stage.rs rename to hostcontrol/src/stage.rs index 2c98cda..8a4901b 100644 --- a/hostcontrol/src/openflexure_stage.rs +++ b/hostcontrol/src/stage.rs @@ -1,7 +1,8 @@ //! Stage controller for openflexure-delta like stage controllers. -use crate::stage_io::StageIO; -use crate::xy_stage::{Result, StageError, XYStage}; +use log::debug; +use std::time::Duration; +use thiserror::Error; fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3 { nalgebra::matrix![ @@ -22,14 +23,74 @@ fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra:: ] } -pub struct OpenFlexureStage { +#[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 { stage_io: StageIO, delta_pos: nalgebra::Vector3, cartesian_to_delta: nalgebra::Matrix3, delta_to_cartesian: nalgebra::Matrix3, } -impl OpenFlexureStage { +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 { 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() @@ -81,19 +142,7 @@ impl OpenFlexureStage { self.move_relative_delta(diff) } - 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() + pub fn move_absolute_cartesian(&mut self, a: nalgebra::Vector3) -> 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]; @@ -113,7 +162,12 @@ impl XYStage for OpenFlexureStage { self.move_absolute_delta(target) } - fn get_range(&self) -> nalgebra::Vector3 { - nalgebra::vector![10000.0, 10000.0, 5000.0] + 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")?) } } diff --git a/hostcontrol/src/stage_io.rs b/hostcontrol/src/stage_io.rs deleted file mode 100644 index 1ffca22..0000000 --- a/hostcontrol/src/stage_io.rs +++ /dev/null @@ -1,56 +0,0 @@ -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 deleted file mode 100644 index 1f645c3..0000000 --- a/hostcontrol/src/xy_stage.rs +++ /dev/null @@ -1,20 +0,0 @@ -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; -}