Push current state

This commit is contained in:
zaubentrucker 2025-02-11 21:23:49 +01:00
parent efcc13bea7
commit 62816b749e
5 changed files with 257 additions and 215 deletions

View file

@ -13,6 +13,8 @@ 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"] }

View file

@ -4,11 +4,7 @@ use gilrs::Button::LeftTrigger2;
use gilrs::Button::RightTrigger2;
use gilrs::EventType::*;
use gilrs::Gilrs;
use std::sync::mpsc;
use std::sync::Arc;
use std::sync::Mutex;
use std::thread::sleep;
use std::time::Duration;
#[derive(Debug)]
pub enum Axis {

View file

@ -1,8 +1,9 @@
use std::ops::DerefMut;
use crate::gamepad::Gamepad;
use crate::printer::Printer;
use euclid::{vec3, Vector3D};
use futures::never::Never;
use std::sync::Arc;
use std::sync::{Arc, Mutex};
use std::time::Duration;
use tokio::io::{AsyncRead, AsyncWrite};
use tokio::time::sleep;
@ -33,11 +34,11 @@ 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 async fn jog<T: AsyncRead + AsyncWrite + Sized>(gamepad: Arc<Mutex<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 (setpoint_x, setpoint_y, setpoint_z) = gamepad.lock().unwrap().deref_mut().speed_setpoint(&[]);
let distance: PrinterVec = vec3(
FULL_SCALE_SPEED_XY * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_x as f64),
@ -49,16 +50,14 @@ pub async fn jog<T: AsyncRead + AsyncWrite + Sized>(gamepad: Arc<Gamepad>, mut p
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:?}",

View file

@ -1,38 +1,15 @@
#![warn(rust_2018_idioms)]
use std::os::fd::RawFd;
use futures::never::Never;
use red::gamepad;
use red::jogger;
use red::printer::Printer;
use std::path::Path;
use futures::never::Never;
use tokio_serial::SerialPortInfo;
#[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 {
look_for_printer()
}
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
}
async fn look_for_printer() -> Never {
fn look_for_printer() -> Never {
println!("Entering App");
let serial_ports = tokio_serial::available_ports()
.expect("Could not list serial ports")
@ -46,9 +23,8 @@ async fn look_for_printer() -> Never {
};
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 printer = Printer::connect_to_path(&port_path.as_os_str().to_string_lossy()).unwrap();
loop {
println!("{}", printer.printer_state());
}
}

View file

@ -2,18 +2,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;
use regex::Regex;
use serialport::SerialPort;
use serialport::TTYPort;
use std::io::Read;
use std::io::Write;
use std::ops::{Deref, DerefMut, Index};
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 std::{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};
@ -29,6 +34,12 @@ 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),
@ -62,110 +73,94 @@ pub enum MovementMode {
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>(
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)
}
}
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(200);
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()`
}));
//TODO: Spawn IO-Thread
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 +168,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
@ -186,7 +181,7 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
}
pub fn printer_state(&self) -> &State {
&self.state
&self.state.lock().unwrap().deref().clone()
}
/// The maximum capacity of the machines GCODE buffer.
@ -197,7 +192,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 +214,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, false, DEFAULT_COMMAND_TIMEOUT)?;
self.state.lock().unwrap().deref_mut().position = res;
Ok(res)
}
@ -231,9 +226,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, true, DEFAULT_COMMAND_TIMEOUT)
}
/// Switch the printer to relative movement mode.
@ -242,9 +237,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_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, true, DEFAULT_COMMAND_TIMEOUT)
}
/// Home the printer using the hardware endstops
@ -252,9 +247,9 @@ impl<S: AsyncRead + AsyncWrite + Sized> Printer<S> {
/// # 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 {
pub 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)?;
self.state.lock().unwrap().deref_mut().position = PrinterPosition {
x: 0.0,
y: 0.0,
z: 0.0,
@ -272,7 +267,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 +281,20 @@ 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?;
if let MovementMode::AbsoluteMovements =
self.state.lock().unwrap().deref_mut().movement_mode
{
self.use_relative_movements()?;
let res = self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT);
self.use_absolute_movements()?;
res
} else {
self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT).await
self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT)
}?;
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 +307,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 +321,155 @@ 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?;
if let MovementMode::RelativeMovements = self.state.lock().unwrap().deref().movement_mode {
self.use_absolute_movements()?;
let res = self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT);
self.use_relative_movements()?;
res
} else {
self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT).await
self.send_gcode(command, true, DEFAULT_COMMAND_TIMEOUT)
}?;
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()),
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>>) {
todo!()
}
/// Read a line from `port` unless it timeouts
///
/// Parameters
/// * `port` - The port to read from
/// * `already_read` - Read bytes from a previous call. May not contain `\n`
/// * `timeout` - Time to wait before raising a timeout error
///
/// Returns
/// `(potential_line, rest)`, where
/// - `potential_line` is `None` if no complete line was read or `Some(line)` if a whole
/// line was received from the port
/// - `rest` contains the bytes that were read since the last occurence of `\n`
fn read_line(
port: &mut TTYPort,
mut already_read: Vec<u8>,
timeout: Duration,
) -> io::Result<(Option<Vec<u8>>, Vec<u8>)> {
let deadline = Instant::now() + timeout;
while Instant::now() < deadline {
match port.read(&mut already_read) {
Err(e) => {
if let io::ErrorKind::TimedOut = e.kind() {
continue;
} else {
return Err(e);
}
}
Ok(0) => panic!("TTYPort returned 0 bytes!"),
Ok(n) => {
if let Some(line_break_idx) = already_read[..n].iter().position(|x| *x == b'\n') {
return Ok((
Some(already_read[..line_break_idx].into()),
already_read[line_break_idx..].into(),
));
}
}
}
}
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>>) {
port.write(user_command.as_bytes())
.expect("Printer IO-Thread hung up its incoming mpsc");
port.flush().unwrap();
let ignore_pos = false;
let mut reply = String::with_capacity(RECV_BUFFER_CAPACITY);
let mut already_read = Vec::new();
loop {
let (mut line, mut rest) = read_line(port, Vec::new(), DEFAULT_COMMAND_TIMEOUT).unwrap();
let timeout = Instant::now() + DEFAULT_COMMAND_TIMEOUT;
while line.is_none() && Instant::now() < timeout {
let buf = Vec::new();
_ = port.read(&mut buf);
if line.is_none() {
std::thread::sleep(Duration::from_millis(5));
}
}
if let Some(line) = line {
let line = String::from_utf8(line).unwrap();
if line.starts_with("ok") {
(*state.lock().unwrap()).last_buffer_capacity = Printer::parse_ok(&line)?;
let reply = command_parser(&reply);
return reply.map_err(PrinterError::GcodeReply);
} else {
reply.push_str(&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 mut 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)
}
}