Compare commits
8 commits
9a450cfe9c
...
6c6ab0e2d7
Author | SHA1 | Date | |
---|---|---|---|
6c6ab0e2d7 | |||
d84ed407ac | |||
2e909d6217 | |||
0199799633 | |||
60aea0a4f5 | |||
c616279c12 | |||
62816b749e | |||
efcc13bea7 |
5 changed files with 380 additions and 292 deletions
|
@ -6,13 +6,12 @@ edition = "2021"
|
|||
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
|
||||
|
||||
[dependencies]
|
||||
bytes = "1.2.1"
|
||||
euclid = "0.22.7"
|
||||
futures = "0.3.24"
|
||||
gilrs = "0.10.1"
|
||||
i2c-linux = "0.1.2"
|
||||
lazy_static = "1.4.0"
|
||||
regex = "1.6.0"
|
||||
serialport = "4.6.1"
|
||||
tokio = { version = "1.21.0", features = ["full"] }
|
||||
tokio-serial = "5.4.3"
|
||||
tokio-util = { version = "0.7.4", features = ["codec"] }
|
||||
|
||||
|
|
|
@ -4,11 +4,7 @@ use gilrs::Button::LeftTrigger2;
|
|||
use gilrs::Button::RightTrigger2;
|
||||
use gilrs::EventType::*;
|
||||
use gilrs::Gilrs;
|
||||
use std::sync::Arc;
|
||||
use std::sync::Mutex;
|
||||
use std::thread::sleep;
|
||||
use std::time::Duration;
|
||||
use tokio::sync::mpsc;
|
||||
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum Axis {
|
||||
|
@ -30,10 +26,12 @@ pub enum GamepadEvent {
|
|||
/// Example:
|
||||
/// ```rust
|
||||
///
|
||||
/// let jogger = jogger::Jogger::new().await.unwrap();
|
||||
/// use red::gamepad;
|
||||
/// let mut jogger = gamepad::Gamepad::new().unwrap();
|
||||
/// loop {
|
||||
/// tokio::time::sleep(Duration::from_secs(2)).await;
|
||||
/// let setpoint = jogger.speed_setpoint();
|
||||
/// std::thread::sleep(std::time::Duration::from_secs(2));
|
||||
/// let events = jogger.get_pending();
|
||||
/// let setpoint = jogger.speed_set_point(&events);
|
||||
/// println!(
|
||||
/// "speed setpoint: {} {} {}",
|
||||
/// setpoint.0, setpoint.1, setpoint.2
|
||||
|
@ -41,73 +39,69 @@ pub enum GamepadEvent {
|
|||
/// }
|
||||
/// ```
|
||||
pub struct Gamepad {
|
||||
speed_setpoint: Mutex<(f32, f32, f32)>,
|
||||
gilrs: Gilrs,
|
||||
// Cached speed setpoint for (X, Y, Positive-Z, Negative-Z)
|
||||
speed_setpoint: (f32, f32, f32, f32),
|
||||
}
|
||||
|
||||
impl Gamepad {
|
||||
/// Create a new `Gamepad` and spawn the underlying tasks for updating internal setpoints
|
||||
///
|
||||
/// The tasks are terminated on drop.
|
||||
pub async fn new() -> Result<Arc<Self>, gilrs::Error> {
|
||||
let (gamepad_tx, gamepad_rx) = mpsc::channel(8);
|
||||
let res = Arc::new(Gamepad {
|
||||
speed_setpoint: Mutex::new((0.0, 0.0, 0.0)),
|
||||
});
|
||||
tokio::task::spawn_blocking(move || {
|
||||
let mut gilrs = Gilrs::new().unwrap();
|
||||
for (_id, gamepad) in gilrs.gamepads() {
|
||||
println!("{} is {:?}", gamepad.name(), gamepad.power_info());
|
||||
}
|
||||
loop {
|
||||
sleep(Duration::from_millis(1));
|
||||
if let Some(event) = gilrs.next_event() {
|
||||
if let Some(internal_event) = Self::map_event(event) {
|
||||
if gamepad_tx.blocking_send(internal_event).is_err() {
|
||||
// receiver dropped
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
});
|
||||
pub fn new() -> Result<Self, gilrs::Error> {
|
||||
let gilrs = Gilrs::new().unwrap();
|
||||
for (_id, gamepad) in gilrs.gamepads() {
|
||||
println!(
|
||||
"Found gamepad {}: {:?}",
|
||||
gamepad.name(),
|
||||
gamepad.power_info()
|
||||
);
|
||||
}
|
||||
|
||||
tokio::spawn(Self::handle_events(res.clone(), gamepad_rx));
|
||||
|
||||
Ok(res)
|
||||
Ok(Self {
|
||||
gilrs,
|
||||
speed_setpoint: (0.0, 0.0, 0.0, 0.0),
|
||||
})
|
||||
}
|
||||
|
||||
/// Get the current speed that the user is dialing in on the gamepad
|
||||
pub fn speed_setpoint(&self) -> (f32, f32, f32) {
|
||||
*self.speed_setpoint.lock().unwrap()
|
||||
fn get_next(&mut self) -> Option<GamepadEvent> {
|
||||
self.gilrs
|
||||
.next_event()
|
||||
.and_then(|event| Self::map_event(event))
|
||||
}
|
||||
|
||||
/// Update the setpoints in accordance to incoming `GamepadEvent`s
|
||||
async fn handle_events(self_arc: Arc<Self>, mut events_rx: mpsc::Receiver<GamepadEvent>) {
|
||||
// There is two z-axes that counter each other.
|
||||
let mut z_positive = 0.0;
|
||||
let mut z_negative = 0.0;
|
||||
while let Some(event) = events_rx.recv().await {
|
||||
/// Get all `GamepadEvent`s since the last call
|
||||
pub fn get_pending(&mut self) -> Vec<GamepadEvent> {
|
||||
(0..).map_while(|_| self.get_next()).collect()
|
||||
}
|
||||
|
||||
/// Update the current speed via the movements on the controller and return the current set
|
||||
/// point
|
||||
///
|
||||
/// If you don't want to update, just supply an empty `updates` slice
|
||||
pub fn speed_set_point(&mut self, updates: &[GamepadEvent]) -> (f32, f32, f32) {
|
||||
for event in updates {
|
||||
match event {
|
||||
GamepadEvent::TerminatePressed => break,
|
||||
GamepadEvent::AxisPosition(axis, value) => {
|
||||
// I won't panic if you don't panic!
|
||||
let mut speed_setpoint = self_arc.speed_setpoint.lock().unwrap();
|
||||
match axis {
|
||||
Axis::X => speed_setpoint.0 = value,
|
||||
Axis::Y => speed_setpoint.1 = value,
|
||||
Axis::X => self.speed_setpoint.0 = *value,
|
||||
Axis::Y => self.speed_setpoint.1 = *value,
|
||||
Axis::ZPositive => {
|
||||
z_positive = value;
|
||||
speed_setpoint.2 = z_positive - z_negative;
|
||||
self.speed_setpoint.2 = *value;
|
||||
}
|
||||
Axis::ZNegative => {
|
||||
z_negative = value;
|
||||
speed_setpoint.2 = z_positive - z_negative;
|
||||
self.speed_setpoint.3 = *value;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
println!("Game pad quit!")
|
||||
(
|
||||
self.speed_setpoint.0,
|
||||
self.speed_setpoint.1,
|
||||
self.speed_setpoint.2 - self.speed_setpoint.3,
|
||||
)
|
||||
}
|
||||
|
||||
fn map_event(event: gilrs::Event) -> Option<GamepadEvent> {
|
||||
|
|
|
@ -2,10 +2,7 @@ use crate::gamepad::Gamepad;
|
|||
use crate::printer::Printer;
|
||||
use euclid::{vec3, Vector3D};
|
||||
use futures::never::Never;
|
||||
use std::sync::Arc;
|
||||
use std::time::Duration;
|
||||
use tokio::io::{AsyncRead, AsyncWrite};
|
||||
use tokio::time::sleep;
|
||||
|
||||
/// Time that a single movement command should take
|
||||
///
|
||||
|
@ -33,11 +30,12 @@ pub struct PrinterUnits;
|
|||
pub type PrinterVec = Vector3D<f64, PrinterUnits>;
|
||||
|
||||
/// Jog the gantry by pumping loads of gcode into the printer board
|
||||
pub async fn jog<T: AsyncRead + AsyncWrite + Sized>(gamepad: Arc<Gamepad>, mut printer: Printer<T>) -> Never {
|
||||
printer.use_absolute_movements().await.unwrap();
|
||||
pub fn jog(gamepad: &mut Gamepad, mut printer: Printer) -> Never {
|
||||
printer.use_absolute_movements().unwrap();
|
||||
println!("Using absolute movements");
|
||||
loop {
|
||||
let (setpoint_x, setpoint_y, setpoint_z) = gamepad.speed_setpoint();
|
||||
let events = gamepad.get_pending();
|
||||
let (setpoint_x, setpoint_y, setpoint_z) = gamepad.speed_set_point(&events);
|
||||
|
||||
let distance: PrinterVec = vec3(
|
||||
FULL_SCALE_SPEED_XY * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_x as f64),
|
||||
|
@ -45,20 +43,18 @@ pub async fn jog<T: AsyncRead + AsyncWrite + Sized>(gamepad: Arc<Gamepad>, mut p
|
|||
FULL_SCALE_SPEED_Z * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_z as f64),
|
||||
);
|
||||
if distance.length() == 0.0 {
|
||||
sleep(TIME_PER_MOVEMENT).await;
|
||||
std::thread::sleep(TIME_PER_MOVEMENT);
|
||||
continue;
|
||||
}
|
||||
let velocity = distance.length() / (TIME_PER_MOVEMENT.as_secs_f64() / 60.0);
|
||||
let old_postion = printer.state.position;
|
||||
let old_postion = printer.state.lock().unwrap().position;
|
||||
printer
|
||||
.move_absolute(
|
||||
old_postion.x + distance.x,
|
||||
old_postion.y + distance.y,
|
||||
old_postion.z + distance.z,
|
||||
velocity.into(),
|
||||
)
|
||||
.await
|
||||
.expect("Failed to send movement command!");
|
||||
).expect("Failed to send movement command!");
|
||||
|
||||
println!(
|
||||
"New position {pos:?}",
|
||||
|
@ -71,9 +67,9 @@ pub async fn jog<T: AsyncRead + AsyncWrite + Sized>(gamepad: Arc<Gamepad>, mut p
|
|||
println!("fill level: {}", fill_level);
|
||||
match fill_level.cmp(&TARGET_BUFER_FILL_LEVEL) {
|
||||
std::cmp::Ordering::Equal => {
|
||||
sleep(LONG_COMMAND_DELAY / SHORT_COMMAND_DELAY_DIVIDER).await
|
||||
std::thread::sleep(LONG_COMMAND_DELAY / SHORT_COMMAND_DELAY_DIVIDER)
|
||||
}
|
||||
std::cmp::Ordering::Greater => sleep(LONG_COMMAND_DELAY).await,
|
||||
std::cmp::Ordering::Greater => std::thread::sleep(LONG_COMMAND_DELAY),
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1,54 +1,49 @@
|
|||
#![warn(rust_2018_idioms)]
|
||||
|
||||
use std::os::fd::RawFd;
|
||||
use futures::never::Never;
|
||||
use red::gamepad;
|
||||
use red::gamepad::Gamepad;
|
||||
use red::jogger;
|
||||
use red::printer::Printer;
|
||||
use std::path::Path;
|
||||
use tokio_serial::SerialPortInfo;
|
||||
use std::path::{Path};
|
||||
|
||||
#[tokio::main]
|
||||
async fn main() -> Never {
|
||||
let args: Vec<String> = std::env::args().collect();
|
||||
|
||||
if args.len() > 1 {
|
||||
inherit_printer().await
|
||||
}
|
||||
else {
|
||||
look_for_printer().await
|
||||
}
|
||||
|
||||
fn main() -> Never {
|
||||
jog()
|
||||
}
|
||||
|
||||
async fn inherit_printer() -> Never {
|
||||
let args: Vec<String> = std::env::args().collect();
|
||||
|
||||
let fd: RawFd = args[1].parse().expect("Not a valid FD");
|
||||
let gamepad = gamepad::Gamepad::new().await.unwrap();
|
||||
let printer = Printer::connect_to_raw_fd(fd)
|
||||
.await
|
||||
.unwrap();
|
||||
jogger::jog(gamepad, printer).await
|
||||
}
|
||||
fn jog() -> Never {
|
||||
let mut gamepad = Gamepad::new().expect("Failed to open gamepad");
|
||||
|
||||
async fn look_for_printer() -> Never {
|
||||
println!("Entering App");
|
||||
let serial_ports = tokio_serial::available_ports()
|
||||
.expect("Could not list serial ports")
|
||||
.into_iter()
|
||||
.filter(|port| port.port_name.contains("ttyUSB"))
|
||||
.collect::<Vec<SerialPortInfo>>();
|
||||
let port = match serial_ports.len() {
|
||||
1 => serial_ports.to_owned().pop().unwrap(),
|
||||
0 => panic!("No USB serial port found! Is the green board disconnected or turned off?"),
|
||||
_ => panic!("There are multiple USB serial ports and we have no way to check which one is the Marlin board!")
|
||||
};
|
||||
let port_path = Path::new("/dev").join(Path::new(&port.port_name).file_name().unwrap());
|
||||
println!("Found serial port: {:?}", &port_path);
|
||||
let gamepad = gamepad::Gamepad::new().await.unwrap();
|
||||
let printer = Printer::connect_to_path(&port_path.as_os_str().to_string_lossy())
|
||||
.await
|
||||
.unwrap();
|
||||
jogger::jog(gamepad, printer).await
|
||||
let dev_dir = std::fs::read_dir(Path::new("/dev")).expect("Failed to open device directory");
|
||||
let mut usb_tty_ports: Vec<String> = dev_dir
|
||||
.filter_map(|entry| {
|
||||
entry
|
||||
.ok()
|
||||
.and_then(|entry| entry.path().to_str().map(|s| s.to_string()))
|
||||
.filter(|path| path.contains("ttyUSB"))
|
||||
})
|
||||
.collect();
|
||||
|
||||
if usb_tty_ports.len() > 1 {
|
||||
eprintln!("Found more than one ttyUSB port! Picking the first one...")
|
||||
}
|
||||
let port_path = usb_tty_ports.pop().expect("No USB serial port found!");
|
||||
|
||||
eprintln!("Found serial port: {:?}", &port_path);
|
||||
|
||||
let printer = Printer::connect_to_path(
|
||||
&port_path
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
jogger::jog(&mut gamepad, printer)
|
||||
}
|
||||
|
||||
fn print_gamepad_events() -> Never {
|
||||
let mut gamepad = Gamepad::new().expect("Failed to open gamepad");
|
||||
loop {
|
||||
let events = gamepad.get_pending();
|
||||
println!("speed setpoint: {:?}", gamepad.speed_set_point(&events));
|
||||
std::thread::sleep(std::time::Duration::from_millis(500));
|
||||
}
|
||||
}
|
|
@ -1,21 +1,23 @@
|
|||
pub mod gcode;
|
||||
use lazy_static::lazy_static;
|
||||
|
||||
use bytes::BytesMut;
|
||||
use futures::sink::SinkExt;
|
||||
use futures::stream::{SplitSink, SplitStream, StreamExt};
|
||||
use core::panic;
|
||||
pub use gcode::{GcodeCommand};
|
||||
use regex::Regex;
|
||||
use serialport::SerialPort;
|
||||
use serialport::TTYPort;
|
||||
use std::io::Read;
|
||||
use std::io::Write;
|
||||
use std::ops::{Deref, DerefMut};
|
||||
use std::os::fd::{FromRawFd, RawFd};
|
||||
use std::sync::mpsc;
|
||||
use std::sync::mpsc::Receiver;
|
||||
use std::sync::mpsc::Sender;
|
||||
use std::sync::mpsc::TryRecvError;
|
||||
use std::sync::Arc;
|
||||
use std::sync::Mutex;
|
||||
use std::time::{Duration, Instant};
|
||||
use std::{fmt::Write, io, str};
|
||||
use tokio;
|
||||
use tokio::fs::File;
|
||||
use tokio::io::{AsyncRead, AsyncWrite};
|
||||
use tokio::time::timeout;
|
||||
use tokio_serial::{SerialPortBuilderExt, SerialStream};
|
||||
use tokio_util::codec::{Decoder, Encoder, Framed};
|
||||
|
||||
pub use gcode::{GcodeCommand, GcodeReply};
|
||||
use std::{io, str};
|
||||
|
||||
use crate::printer::gcode::{G91Command, M114Command};
|
||||
|
||||
|
@ -24,11 +26,17 @@ use self::gcode::{G0Command, G28Command, G90Command, GcodeReplyError};
|
|||
/// Recv buffer string will be initialized with this capacity.
|
||||
/// This should fit a simple "OK Pnn Bn" reply for GCODE commands that
|
||||
/// do not return any data.
|
||||
const RECV_BUFFER_CAPACITY: usize = 32;
|
||||
const BUF_SIZE_READ_LINE: usize = 1024;
|
||||
const BAUD_RATE: u32 = 115200;
|
||||
|
||||
const DEFAULT_COMMAND_TIMEOUT: Duration = Duration::from_secs(2);
|
||||
|
||||
/// Timeout for reads and writes to the serial port
|
||||
const SERIALPORT_TIMEOUT: Duration = Duration::from_millis(10);
|
||||
|
||||
/// Time the IO-thread may take to reply to the user thread after issuing a command
|
||||
const IO_THREAD_TIMEOUT: Duration = Duration::from_millis(300);
|
||||
|
||||
pub enum Port {
|
||||
OpenFd(RawFd),
|
||||
Path(String),
|
||||
|
@ -54,118 +62,98 @@ pub struct PrinterPosition {
|
|||
pub z: f64,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub enum MovementMode {
|
||||
AbsoluteMovements,
|
||||
RelativeMovements,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct State {
|
||||
pub position: PrinterPosition,
|
||||
pub movement_mode: MovementMode,
|
||||
pub last_buffer_capacity: usize,
|
||||
}
|
||||
|
||||
pub struct Printer<T: AsyncRead + AsyncWrite + Sized> {
|
||||
pub state: State,
|
||||
serial_tx: SplitSink<Framed<T, LineCodec>, String>,
|
||||
serial_rx: SplitStream<Framed<T, LineCodec>>,
|
||||
last_buffer_capacity: usize,
|
||||
pub struct Printer {
|
||||
pub state: Arc<Mutex<State>>,
|
||||
/// Queue a command to be sent to the IO thread
|
||||
to_io_thread: std::sync::mpsc::Sender<String>,
|
||||
/// Used read replies as received from the printer. Should not be written to
|
||||
from_io_thread: std::sync::mpsc::Receiver<Vec<u8>>,
|
||||
maximum_buffer_capacity: usize,
|
||||
}
|
||||
|
||||
impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
||||
impl Printer {
|
||||
/// Send gcode to the printer and parse its reply
|
||||
async fn send_gcode<T: GcodeCommand>(
|
||||
&mut self,
|
||||
command: T,
|
||||
ignore_pos: bool,
|
||||
timeout: Duration,
|
||||
) -> Result<T::Reply, PrinterError> {
|
||||
let command_text = format!("{}\n", command.command());
|
||||
self.serial_tx.send(command_text.clone()).await.unwrap();
|
||||
self.serial_tx.flush().await.unwrap();
|
||||
let mut reply = String::with_capacity(RECV_BUFFER_CAPACITY);
|
||||
loop {
|
||||
let mut line = None;
|
||||
let timeout = Instant::now() + timeout;
|
||||
while line.is_none() && Instant::now() < timeout {
|
||||
line = self.serial_rx.next().await;
|
||||
if line.is_none() {
|
||||
tokio::time::sleep(Duration::from_millis(5)).await;
|
||||
}
|
||||
}
|
||||
if let Some(line) = line {
|
||||
match line {
|
||||
Ok(line) => {
|
||||
if ignore_pos && (line.starts_with(" T:") || line.starts_with("X:")) {
|
||||
continue;
|
||||
}
|
||||
if line.contains("ok") {
|
||||
self.last_buffer_capacity = Self::parse_ok(&line)?;
|
||||
let reply = command.parse_reply(&reply);
|
||||
return reply.map_err(PrinterError::GcodeReply);
|
||||
} else {
|
||||
reply.push_str(&line);
|
||||
}
|
||||
}
|
||||
Err(e) => {
|
||||
println!("Failed to read from Printer: {}", e)
|
||||
}
|
||||
}
|
||||
fn send_gcode<T: GcodeCommand>(&mut self, command: T) -> Result<T::Reply, PrinterError> {
|
||||
let command_text = command.command() + "\n";
|
||||
self.to_io_thread
|
||||
.send(command_text.clone())
|
||||
.expect("Printer IO-Thread hung up its incoming mpsc");
|
||||
let reply = self.from_io_thread.recv_timeout(IO_THREAD_TIMEOUT);
|
||||
match reply {
|
||||
Ok(reply) => Ok(command
|
||||
.parse_reply(&String::from_utf8(reply).expect("Invalid UTF-8 reply from printer"))
|
||||
.expect("Could not parse reply from printer")),
|
||||
|
||||
Err(e) => {
|
||||
panic!("Printer didn't reply in time: {}", e)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn connect(port: S) -> Result<Self, PrinterError> {
|
||||
let connection = LineCodec.framed(port);
|
||||
let (serial_tx, mut serial_rx) = connection.split();
|
||||
|
||||
pub fn connect(mut port: TTYPort) -> Result<Self, PrinterError> {
|
||||
// The printer will send some info after connecting for the first time. We need to wait for this
|
||||
// to be received as it will otherwise stop responding for some reason:
|
||||
loop {
|
||||
if let Ok(message) = timeout(Duration::from_millis(100), serial_rx.next()).await {
|
||||
match message {
|
||||
Some(Ok(reply)) => {
|
||||
println!("got stuff: {:?}", reply);
|
||||
if reply.contains("Loaded") {
|
||||
break;
|
||||
}
|
||||
if reply.contains("ok") {
|
||||
break;
|
||||
}
|
||||
if reply.starts_with("X:") {
|
||||
break;
|
||||
}
|
||||
}
|
||||
Some(Err(e)) => {
|
||||
println!("Error reading from serial port: {:?}", e);
|
||||
}
|
||||
None => (),
|
||||
|
||||
port.set_timeout(SERIALPORT_TIMEOUT)
|
||||
.expect("Cannot set serial port timeout");
|
||||
|
||||
let mut buf = [0; 1024];
|
||||
let deadline = Instant::now() + Duration::from_millis(2000);
|
||||
let mut initial_msg = Vec::new();
|
||||
while Instant::now() < deadline {
|
||||
if let Ok(message) = port.read(&mut buf) {
|
||||
if message > 0 {
|
||||
initial_msg.extend(&buf[0..message]);
|
||||
}
|
||||
} else {
|
||||
println!(
|
||||
"Reading from serial port timed out. Printer might already be initialized."
|
||||
);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
let mut res = Printer {
|
||||
serial_rx,
|
||||
serial_tx,
|
||||
state: State {
|
||||
position: PrinterPosition {
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
z: 0.0,
|
||||
},
|
||||
movement_mode: MovementMode::AbsoluteMovements,
|
||||
if let Ok(initial_msg) = String::from_utf8(initial_msg.clone()) {
|
||||
println!("Initial message from printer was:\n{initial_msg}")
|
||||
} else {
|
||||
println!("Initial message from printer was not valid UTF8: {initial_msg:?}")
|
||||
}
|
||||
|
||||
let (to_io_thread, from_user_thread) = mpsc::channel();
|
||||
let (to_user_thread, from_io_thread) = mpsc::channel();
|
||||
|
||||
let state = Arc::new(Mutex::new(State {
|
||||
position: PrinterPosition {
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
z: 0.0,
|
||||
},
|
||||
movement_mode: MovementMode::AbsoluteMovements,
|
||||
last_buffer_capacity: 0, // this is updated on the next call to `send_gcode()`
|
||||
}));
|
||||
|
||||
let state_for_io = state.clone();
|
||||
std::thread::spawn(move || {
|
||||
Self::io_thread_work(to_user_thread, from_user_thread, port, state_for_io)
|
||||
});
|
||||
|
||||
let mut res = Printer {
|
||||
from_io_thread,
|
||||
to_io_thread,
|
||||
state: state.clone(),
|
||||
maximum_buffer_capacity: 0, // this is updated on the next call to `send_gcode()`
|
||||
};
|
||||
|
||||
// This implicitly sets `res.last_buffer_capacity`
|
||||
res.use_absolute_movements().await.map_err(|err| {
|
||||
res.use_absolute_movements().map_err(|err| {
|
||||
PrinterError::InitializationError(format!(
|
||||
"Failed to set absolute movements mode: {:?}",
|
||||
err
|
||||
|
@ -173,9 +161,9 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
})?;
|
||||
|
||||
// since we never sent any positioning GCODE, we should be at max-capacity now.
|
||||
res.maximum_buffer_capacity = res.last_buffer_capacity;
|
||||
res.maximum_buffer_capacity = res.state.lock().unwrap().deref().last_buffer_capacity;
|
||||
|
||||
res.update_position().await.map_err(|err| {
|
||||
res.update_position().map_err(|err| {
|
||||
PrinterError::InitializationError(format!(
|
||||
"Failed to get the current position: {:?}",
|
||||
err
|
||||
|
@ -185,8 +173,8 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
Ok(res)
|
||||
}
|
||||
|
||||
pub fn printer_state(&self) -> &State {
|
||||
&self.state
|
||||
pub fn printer_state(&self) -> State {
|
||||
self.state.lock().unwrap().deref().clone()
|
||||
}
|
||||
|
||||
/// The maximum capacity of the machines GCODE buffer.
|
||||
|
@ -197,7 +185,7 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
/// The remaining capacity of the machines GCODE buffer.
|
||||
/// This value is refreshed after each sent command.
|
||||
pub fn remaining_capacity(&self) -> usize {
|
||||
self.last_buffer_capacity
|
||||
self.state.lock().unwrap().deref().last_buffer_capacity
|
||||
}
|
||||
|
||||
/// Parse the "Ok" confirmation line that the printer sends after every successfully received
|
||||
|
@ -219,9 +207,9 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
}
|
||||
|
||||
/// Update the internal position by asking the printer for it
|
||||
async fn update_position(&mut self) -> Result<PrinterPosition, PrinterError> {
|
||||
let res = self.send_gcode(M114Command, false, DEFAULT_COMMAND_TIMEOUT).await?;
|
||||
self.state.position = res;
|
||||
fn update_position(&mut self) -> Result<PrinterPosition, PrinterError> {
|
||||
let res = self.send_gcode(M114Command)?;
|
||||
self.state.lock().unwrap().deref_mut().position = res;
|
||||
Ok(res)
|
||||
}
|
||||
|
||||
|
@ -231,9 +219,9 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
/// Otherwise, the motion planner on the printer will stop the printer between all movements
|
||||
/// as `self.move_absolute()` will call `use_absolute_movements` and `use_relative_movements`
|
||||
/// itself. (See its documentation)
|
||||
pub async fn use_absolute_movements(&mut self) -> Result<(), PrinterError> {
|
||||
self.state.movement_mode = MovementMode::AbsoluteMovements;
|
||||
self.send_gcode(G90Command, true, DEFAULT_COMMAND_TIMEOUT).await
|
||||
pub fn use_absolute_movements(&mut self) -> Result<(), PrinterError> {
|
||||
self.state.lock().unwrap().deref_mut().movement_mode = MovementMode::AbsoluteMovements;
|
||||
self.send_gcode(G90Command)
|
||||
}
|
||||
|
||||
/// Switch the printer to relative movement mode.
|
||||
|
@ -242,19 +230,19 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
/// Otherwise, the motion planner on the printer will stop the printer between all movements
|
||||
/// as `self.move_relative()` will call `use_absolute_movements` and `use_relative_movements`
|
||||
/// itself. (See its documentation)
|
||||
pub async fn use_relative_movements(&mut self) -> Result<(), PrinterError> {
|
||||
self.state.movement_mode = MovementMode::RelativeMovements;
|
||||
self.send_gcode(G91Command, true, DEFAULT_COMMAND_TIMEOUT).await
|
||||
pub fn use_relative_movements(&mut self) -> Result<(), PrinterError> {
|
||||
self.state.lock().unwrap().deref_mut().movement_mode = MovementMode::RelativeMovements;
|
||||
self.send_gcode(G91Command)
|
||||
}
|
||||
|
||||
/// Home the printer using the hardware endstops
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `x, y, z` - Whether the axis should be homed. Axis that are set to `false` will not be
|
||||
/// homed.,
|
||||
pub async fn auto_home(&mut self, x: bool, y: bool, z: bool) -> Result<(), PrinterError> {
|
||||
self.send_gcode(G28Command::new(x, y, z), true, DEFAULT_COMMAND_TIMEOUT).await?;
|
||||
self.state.position = PrinterPosition {
|
||||
/// homed.
|
||||
pub fn auto_home(&mut self, x: bool, y: bool, z: bool) -> Result<(), PrinterError> {
|
||||
self.send_gcode(G28Command::new(x, y, z))?;
|
||||
self.state.lock().unwrap().deref_mut().position = PrinterPosition {
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
z: 0.0,
|
||||
|
@ -272,7 +260,7 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
/// # Arguments
|
||||
/// * `x, y, z` - Position offset in printer units (mm)
|
||||
/// * `velocity` - Velocity that the printer should move at (mm/min)
|
||||
pub async fn move_relative(
|
||||
pub fn move_relative(
|
||||
&mut self,
|
||||
x: f64,
|
||||
y: f64,
|
||||
|
@ -286,18 +274,19 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
e: None, // Machine has no e
|
||||
velocity,
|
||||
};
|
||||
if let MovementMode::AbsoluteMovements = self.state.movement_mode {
|
||||
self.use_relative_movements().await?;
|
||||
let res = self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT).await;
|
||||
self.use_absolute_movements().await?;
|
||||
let movement_mode = self.state.lock().unwrap().deref_mut().movement_mode;
|
||||
if let MovementMode::AbsoluteMovements = movement_mode {
|
||||
self.use_relative_movements()?;
|
||||
let res = self.send_gcode(command);
|
||||
self.use_absolute_movements()?;
|
||||
res
|
||||
} else {
|
||||
self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT).await
|
||||
self.send_gcode(command)
|
||||
}?;
|
||||
|
||||
self.state.position.x += x;
|
||||
self.state.position.y += y;
|
||||
self.state.position.z += z;
|
||||
self.state.lock().unwrap().deref_mut().position.x += x;
|
||||
self.state.lock().unwrap().deref_mut().position.y += y;
|
||||
self.state.lock().unwrap().deref_mut().position.z += z;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
@ -310,7 +299,7 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
///
|
||||
/// * `x, y, z` - New position in printer units (mm)
|
||||
/// * `velocity` - Velocity that the printer should move at (mm/min)
|
||||
pub async fn move_absolute(
|
||||
pub fn move_absolute(
|
||||
&mut self,
|
||||
x: f64,
|
||||
y: f64,
|
||||
|
@ -324,83 +313,198 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
|
|||
e: None, // Machine has no e
|
||||
velocity,
|
||||
};
|
||||
if let MovementMode::RelativeMovements = self.state.movement_mode {
|
||||
self.use_absolute_movements().await?;
|
||||
let res = self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT).await;
|
||||
self.use_relative_movements().await?;
|
||||
let movement_mode = self.state.lock().unwrap().deref().movement_mode;
|
||||
if let MovementMode::RelativeMovements = movement_mode {
|
||||
self.use_absolute_movements()?;
|
||||
let res = self.send_gcode(command);
|
||||
self.use_relative_movements()?;
|
||||
res
|
||||
} else {
|
||||
self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT).await
|
||||
self.send_gcode(command)
|
||||
}?;
|
||||
|
||||
self.state.position.x = x;
|
||||
self.state.position.y = y;
|
||||
self.state.position.z = z;
|
||||
self.state.lock().unwrap().deref_mut().position.x = x;
|
||||
self.state.lock().unwrap().deref_mut().position.y = y;
|
||||
self.state.lock().unwrap().deref_mut().position.z = z;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Background thread that handles direct communication with the printer serial port
|
||||
///
|
||||
/// Parameters
|
||||
/// * `state` - State that is shared between background thread and the thread that owns the
|
||||
/// according `Printer` struct
|
||||
fn io_thread_work(
|
||||
to_user_thread: Sender<Vec<u8>>,
|
||||
from_user_thread: Receiver<String>,
|
||||
mut port: TTYPort,
|
||||
state: Arc<Mutex<State>>,
|
||||
) {
|
||||
loop {
|
||||
match from_user_thread.try_recv() {
|
||||
Ok(user_command) => handle_user_command(
|
||||
&mut port,
|
||||
user_command,
|
||||
state.clone(),
|
||||
to_user_thread.clone(),
|
||||
),
|
||||
Err(TryRecvError::Disconnected) => break,
|
||||
Err(TryRecvError::Empty) => handle_printer_autoreport(&mut port, state.clone()),
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Printer<SerialStream> {
|
||||
pub async fn connect_to_path(port_path: &str) -> Result<Self, PrinterError> {
|
||||
let mut port = tokio_serial::new(port_path, BAUD_RATE)
|
||||
.open_native_async()
|
||||
/// Check for auto-report messages coming in from the printer and update the `state` accordingly
|
||||
fn handle_printer_autoreport(port: &mut TTYPort, state: Arc<Mutex<State>>) {
|
||||
if port
|
||||
.bytes_to_read()
|
||||
.expect("`handle_printer_autoreport`: Failed to check for available data")
|
||||
> 0
|
||||
{
|
||||
let mut buf = [0; BUF_SIZE_READ_LINE];
|
||||
let bytes_read = port
|
||||
.read(&mut buf)
|
||||
.expect("`handle_printer_autoreport`: Failed to read data");
|
||||
assert!(
|
||||
bytes_read > 0,
|
||||
"The port returned 0 bytes after a read. Is the port closed?"
|
||||
);
|
||||
match String::from_utf8(buf[..bytes_read].to_vec()) {
|
||||
Ok(s) => {
|
||||
println!("Received autoreport:");
|
||||
println!("<< {}", s);
|
||||
}
|
||||
Err(e) => {
|
||||
panic!(
|
||||
"`handle_printer_autoreport` Failed to parse received bytes `{}` as UTF8: {}",
|
||||
bytes_read, e
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Read a line from `port` unless it timeouts
|
||||
///
|
||||
/// Parameters
|
||||
/// * `port` - The port to read from
|
||||
/// * `already_read` - Read bytes from a previous call.
|
||||
/// On a successful read, the buffer will be cleared until the end of the returned line and then
|
||||
/// filled with the new residual bytes. It might contain an entire line if the port returns
|
||||
/// multiple lines in one read. It is fine to then call this function again to retrieve the line.
|
||||
/// * `timeout` - Time to wait for new bytes from the `port` before raising a timeout error
|
||||
///
|
||||
/// Returns
|
||||
///
|
||||
/// * `Ok(Some(line))` if a complete line was read from the port
|
||||
/// * `Ok(None)` if there was no `\n` received
|
||||
/// * `Err(e)` in case reading failed
|
||||
fn read_line(
|
||||
port: &mut TTYPort,
|
||||
already_read: &mut Vec<u8>,
|
||||
timeout: Duration,
|
||||
) -> io::Result<Vec<u8>> {
|
||||
let deadline = Instant::now() + timeout;
|
||||
let mut buf = [0; BUF_SIZE_READ_LINE];
|
||||
while Instant::now() < deadline {
|
||||
if let Some(line_break_idx) = already_read.iter().position(|x| *x == b'\n') {
|
||||
let res = Ok(already_read[..line_break_idx].into());
|
||||
*already_read = already_read[line_break_idx+1..].into();
|
||||
return res;
|
||||
}
|
||||
match port.read(&mut buf) {
|
||||
Err(e) => {
|
||||
if let io::ErrorKind::TimedOut = e.kind() {
|
||||
continue;
|
||||
} else {
|
||||
return Err(e);
|
||||
}
|
||||
}
|
||||
Ok(0) => panic!("TTYPort returned 0 bytes!"),
|
||||
Ok(n) => {
|
||||
already_read.extend_from_slice(&buf[..n]);
|
||||
}
|
||||
}
|
||||
}
|
||||
Err(io::Error::new(
|
||||
io::ErrorKind::TimedOut,
|
||||
"Couldn't read a line within timeout!",
|
||||
))
|
||||
}
|
||||
|
||||
fn handle_user_command(
|
||||
port: &mut TTYPort,
|
||||
user_command: String,
|
||||
state: Arc<Mutex<State>>,
|
||||
to_user_thread: Sender<Vec<u8>>,
|
||||
) {
|
||||
// TODO: Add timeout?
|
||||
println!(">> {}", user_command);
|
||||
port.write_all(user_command.as_bytes())
|
||||
.expect("Failed to write to printer serial port");
|
||||
port.flush().unwrap();
|
||||
|
||||
let mut already_read_lines = Vec::new();
|
||||
let mut rest = Vec::new();
|
||||
let start_time = Instant::now();
|
||||
loop {
|
||||
if start_time.elapsed() > DEFAULT_COMMAND_TIMEOUT {
|
||||
panic!("No reply from printer within timeout");
|
||||
}
|
||||
let line = read_line(port, &mut rest, DEFAULT_COMMAND_TIMEOUT)
|
||||
.expect("Failed to read from printer");
|
||||
|
||||
let str_line = String::from_utf8(line.clone()).expect("Read line was no valid utf8");
|
||||
println!("<< {:?}", str_line);
|
||||
|
||||
if str_line.starts_with("ok") {
|
||||
state.lock().unwrap().last_buffer_capacity =
|
||||
Printer::parse_ok(&str_line).expect("Couldn't parse line as 'ok'-message");
|
||||
to_user_thread
|
||||
.send(already_read_lines.join(&b'\n'))
|
||||
.expect("Failed to send read bytes to user thread");
|
||||
break;
|
||||
} else {
|
||||
already_read_lines.push(line);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Printer {
|
||||
pub fn connect_to_raw_fd(port: RawFd) -> Result<Self, PrinterError> {
|
||||
let port = unsafe { serialport::TTYPort::from_raw_fd(port) };
|
||||
// We can't set port exclusive here as in `connect_to_path()`, since a parent
|
||||
// process might still have the fd open.
|
||||
|
||||
Self::connect(port)
|
||||
}
|
||||
|
||||
pub fn connect_to_path(port_path: &str) -> Result<Self, PrinterError> {
|
||||
let port = serialport::new(port_path, BAUD_RATE)
|
||||
.timeout(Duration::from_millis(100))
|
||||
.open_native()
|
||||
.expect("Unable to open serial port");
|
||||
|
||||
port.set_exclusive(true)
|
||||
.expect("Unable to set serial port exclusive to false");
|
||||
|
||||
Self::connect(port).await
|
||||
}
|
||||
}
|
||||
|
||||
impl Printer<File> {
|
||||
pub async fn connect_to_raw_fd(port: RawFd) -> Result<Self, PrinterError> {
|
||||
let port = unsafe { File::from_raw_fd(port) };
|
||||
Self::connect(port).await
|
||||
}
|
||||
}
|
||||
|
||||
struct LineCodec;
|
||||
|
||||
impl Decoder for LineCodec {
|
||||
type Item = String;
|
||||
type Error = io::Error;
|
||||
|
||||
fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
|
||||
let newline = src.as_ref().iter().position(|b| *b == b'\n');
|
||||
if let Some(n) = newline {
|
||||
let line = src.split_to(n + 1);
|
||||
return match str::from_utf8(line.as_ref()) {
|
||||
Ok(s) => {
|
||||
println!(">>{}", s);
|
||||
Ok(Some(s.to_string()))
|
||||
}
|
||||
Err(_) => Err(io::Error::new(io::ErrorKind::Other, "Invalid String")),
|
||||
};
|
||||
}
|
||||
Ok(None)
|
||||
}
|
||||
}
|
||||
|
||||
impl Encoder<String> for LineCodec {
|
||||
type Error = io::Error;
|
||||
|
||||
fn encode(&mut self, item: String, dst: &mut BytesMut) -> Result<(), Self::Error> {
|
||||
println!("<<{}", item);
|
||||
dst.write_str(&item)
|
||||
.map_err(|e| io::Error::new(io::ErrorKind::InvalidData, e.to_string()))?;
|
||||
Ok(())
|
||||
Self::connect(port)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use super::Printer;
|
||||
use super::{Printer, IO_THREAD_TIMEOUT, SERIALPORT_TIMEOUT};
|
||||
|
||||
#[test]
|
||||
fn test_parse_ok() {
|
||||
let buffer_cap = Printer::parse_ok("ok P10 B4564").unwrap();
|
||||
assert!(buffer_cap == 10);
|
||||
assert_eq!(buffer_cap, 10);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn check_timeout_sanity() {
|
||||
// The user thread will wait for the IO-thread. The IO-thread then needs to have enough time
|
||||
// to recognize a serial port timeout and propagate that to the user thread.
|
||||
assert!(IO_THREAD_TIMEOUT > SERIALPORT_TIMEOUT)
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Add table
Reference in a new issue