Run cargo fmt
... to have canonical formatting. Signed-off-by: Henner Zeller <h.zeller@acm.org>
This commit is contained in:
parent
7d8337bd5e
commit
4e946eef77
2 changed files with 45 additions and 22 deletions
|
|
@ -1,5 +1,5 @@
|
||||||
use std::{thread::sleep, time::Duration};
|
|
||||||
use clap::Parser;
|
use clap::Parser;
|
||||||
|
use std::{thread::sleep, time::Duration};
|
||||||
|
|
||||||
mod stage;
|
mod stage;
|
||||||
|
|
||||||
|
|
@ -7,11 +7,11 @@ mod stage;
|
||||||
#[command(version, about, long_about = None)]
|
#[command(version, about, long_about = None)]
|
||||||
struct CliArgs {
|
struct CliArgs {
|
||||||
/// Interface to talk to movement stage
|
/// Interface to talk to movement stage
|
||||||
#[arg(long, default_value="/dev/ttyACM0")]
|
#[arg(long, default_value = "/dev/ttyACM0")]
|
||||||
stage_device: String,
|
stage_device: String,
|
||||||
|
|
||||||
/// Speed of the stage device (bps)
|
/// Speed of the stage device (bps)
|
||||||
#[arg(long, default_value="115200")]
|
#[arg(long, default_value = "115200")]
|
||||||
tty_speed: u32,
|
tty_speed: u32,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -43,8 +43,7 @@ fn main() {
|
||||||
//Ok(())
|
//Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
struct App {
|
struct App {}
|
||||||
}
|
|
||||||
|
|
||||||
impl Default for App {
|
impl Default for App {
|
||||||
fn default() -> Self {
|
fn default() -> Self {
|
||||||
|
|
|
||||||
|
|
@ -1,8 +1,8 @@
|
||||||
//! Stage controller for openflexure-delta like stage controllers.
|
//! Stage controller for openflexure-delta like stage controllers.
|
||||||
|
|
||||||
|
use log::debug;
|
||||||
use std::time::Duration;
|
use std::time::Duration;
|
||||||
use thiserror::Error;
|
use thiserror::Error;
|
||||||
use log::debug;
|
|
||||||
|
|
||||||
fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
|
fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
|
||||||
nalgebra::matrix![
|
nalgebra::matrix![
|
||||||
|
|
@ -15,7 +15,7 @@ fn new_camera_rotation(theta: f64) -> nalgebra::Matrix3<f64> {
|
||||||
fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra::Matrix3<f64> {
|
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 x_fac = (2.0 / 3.0f64.sqrt()) * (flex_b / flex_h);
|
||||||
let y_fac = -1.0 * flex_b / flex_h;
|
let y_fac = -1.0 * flex_b / flex_h;
|
||||||
let z_fac = 1.0/3.0 * flex_b / flex_a;
|
let z_fac = 1.0 / 3.0 * flex_b / flex_a;
|
||||||
nalgebra::matrix![
|
nalgebra::matrix![
|
||||||
-x_fac, x_fac, 0.0;
|
-x_fac, x_fac, 0.0;
|
||||||
0.5 * y_fac, 0.5 * y_fac, -y_fac;
|
0.5 * y_fac, 0.5 * y_fac, -y_fac;
|
||||||
|
|
@ -55,17 +55,21 @@ pub struct Stage {
|
||||||
pub type StageIOResult<T> = std::result::Result<T, StageIOError>;
|
pub type StageIOResult<T> = std::result::Result<T, StageIOError>;
|
||||||
impl StageIO {
|
impl StageIO {
|
||||||
pub fn new(port: &str, speed: u32) -> StageIOResult<Self> {
|
pub fn new(port: &str, speed: u32) -> StageIOResult<Self> {
|
||||||
let serial = serialport::new(port, speed).timeout(Duration::from_secs(60)).open()?;
|
let serial = serialport::new(port, speed)
|
||||||
// TODO: read and discard initial characters; some devices like to be
|
.timeout(Duration::from_secs(60))
|
||||||
// chatty on start-up.
|
.open()?;
|
||||||
Ok(Self { serial })
|
// TODO: read and discard initial characters; some devices like to be
|
||||||
|
// chatty on start-up.
|
||||||
|
Ok(Self { serial })
|
||||||
}
|
}
|
||||||
|
|
||||||
fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> StageIOResult<String> {
|
fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> StageIOResult<String> {
|
||||||
let mut res: Vec<u8> = vec![];
|
let mut res: Vec<u8> = vec![];
|
||||||
loop {
|
loop {
|
||||||
let mut buf = [0u8; 1];
|
let mut buf = [0u8; 1];
|
||||||
self.serial.read_exact(&mut buf).map_err(|e| StageIOError::Read(e))?;
|
self.serial
|
||||||
|
.read_exact(&mut buf)
|
||||||
|
.map_err(|e| StageIOError::Read(e))?;
|
||||||
res.push(buf[0]);
|
res.push(buf[0]);
|
||||||
if res.ends_with(needle.as_ref().as_bytes()) {
|
if res.ends_with(needle.as_ref().as_bytes()) {
|
||||||
return Ok(String::from_utf8_lossy(&res).to_string());
|
return Ok(String::from_utf8_lossy(&res).to_string());
|
||||||
|
|
@ -75,10 +79,12 @@ impl StageIO {
|
||||||
|
|
||||||
// Send request and wait for response deliminated with a newline
|
// Send request and wait for response deliminated with a newline
|
||||||
pub fn send_request<S: AsRef<str>>(&mut self, command: S) -> StageIOResult<String> {
|
pub fn send_request<S: AsRef<str>>(&mut self, command: S) -> StageIOResult<String> {
|
||||||
debug!("->: {:?}", command.as_ref());
|
debug!("->: {:?}", command.as_ref());
|
||||||
self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageIOError::Write(e))?;
|
self.serial
|
||||||
|
.write_all(command.as_ref().as_bytes())
|
||||||
|
.map_err(|e| StageIOError::Write(e))?;
|
||||||
let response = self.receive_until("\n")?;
|
let response = self.receive_until("\n")?;
|
||||||
debug!("<-: {:?}", response);
|
debug!("<-: {:?}", response);
|
||||||
Ok(response.trim().to_string())
|
Ok(response.trim().to_string())
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -87,28 +93,46 @@ pub type Result<T> = std::result::Result<T, StageError>;
|
||||||
impl Stage {
|
impl Stage {
|
||||||
pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result<Self> {
|
pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result<Self> {
|
||||||
// TODO: non-blocking moves
|
// TODO: non-blocking moves
|
||||||
let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() * new_delta_into_cartesian(80.0, 50.0, 50.0);
|
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 cartesian_to_delta = delta_to_cartesian.try_inverse().unwrap();
|
||||||
let mut res = Self { stage_io, 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.stage_io.send_request("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]];
|
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)
|
Ok(res)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
pub fn get_position_delta(&self) -> nalgebra::Vector3<i32> {
|
pub fn get_position_delta(&self) -> nalgebra::Vector3<i32> {
|
||||||
self.delta_pos.clone()
|
self.delta_pos.clone()
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn get_position_cartesian(&self) -> nalgebra::Vector3<f64> {
|
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
|
self.delta_to_cartesian * pos
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn move_relative_delta(&mut self, d: nalgebra::Vector3<i32>) -> Result<()> {
|
pub fn move_relative_delta(&mut self, d: nalgebra::Vector3<i32>) -> Result<()> {
|
||||||
self.stage_io.send_request(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]];
|
self.delta_pos += nalgebra::vector![d[0], d[1], d[2]];
|
||||||
Ok(())
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue