Use fill level for jogging

red/src/jogger.rs

@@ -1,10 +1,11 @@
 use crate::gamepad::Gamepad;
 use crate::printer::gcode::{G0Command, G91Command};
-use crate::printer::{Printer};
+use crate::printer::Printer;
 use euclid::{vec3, Vector3D};
 use futures::never::Never;
 use std::sync::Arc;
 use std::time::Duration;
+use tokio::time::sleep;
 
 /// Time that a single movement command should take
 ///
@@ -17,17 +18,22 @@ const FULL_SCALE_SPEED_XY: f64 = 1000.0;
 /// Movement speed of gantry when going full throttle in z-direction
 /// in units/min (mm/min)
 const FULL_SCALE_SPEED_Z: f64 = 10.0;
+/// Amount of GCODE buffers that should be filled for optimal jogging performance.
+/// More buffers increase the controller delay. Less buffers make it more difficult
+/// for the motion planner on the printer to plan ahead.
+const TARGET_BUFER_FILL_LEVEL: usize = 4;
 
 /// Should be mm on most machine including the Leapfrog
 pub struct PrinterUnits;
 pub type PrinterVec = Vector3D<f64, PrinterUnits>;
 
-/// Jogging the gantry by pumping loads of gcode into the printer board
+/// Jog the gantry by pumping loads of gcode into the printer board
 pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
     printer.send_gcode(Box::new(G91Command)).await.unwrap();
     println!("Sent G91Command");
     loop {
         let (setpoint_x, setpoint_y, setpoint_z) = gamepad.speed_setpoint();
+
         // We're bound by lower speed on z and have no way to go at separate speeds per axis
         let full_scale_speed = if setpoint_z == 0.0 {
             FULL_SCALE_SPEED_XY
@@ -40,6 +46,7 @@ pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
             full_scale_speed * (TIME_PER_MOVEMENT.as_secs_f64() / 60.0) * (setpoint_z as f64),
         );
         if distance.length() == 0.0 {
+            sleep(TIME_PER_MOVEMENT).await;
             continue;
         }
         let velocity = distance.length() / (TIME_PER_MOVEMENT.as_secs_f64() / 60.0);
@@ -50,6 +57,20 @@ pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
             e: None,
             velocity: velocity.into(),
         };
-        printer.send_gcode(Box::new(command)).await;
+        printer
+            .send_gcode(Box::new(command))
+            .await
+            .expect("Failed to send movement command!");
+
+        // Wait for one command time if buffer is overfull, wait for half that time if buffer is
+        // filled *just* right.
+        let fill_level = printer.maximum_capacity() - printer.remaining_capacity();
+        println!("remaining capacity: {}", printer.remaining_capacity());
+        println!("fill level: {}", fill_level);
+        if fill_level > TARGET_BUFER_FILL_LEVEL {
+            sleep(TIME_PER_MOVEMENT * 2).await;
+        } else if fill_level == TARGET_BUFER_FILL_LEVEL {
+            sleep(TIME_PER_MOVEMENT / 2).await;
+        }
     }
 }

red/src/lib.rs

@@ -1,4 +1,3 @@
-#![warn(missing_docs)]
 pub mod gamepad;
 pub mod jogger;
 pub mod printer;

red/src/main.rs

@@ -1,5 +1,4 @@
 #![warn(rust_2018_idioms)]
-#![warn(missing_docs)]
 use futures::never::Never;
 use i2c_linux::I2c;
 use red::gamepad;

red/src/printer/mod.rs

@@ -1,4 +1,5 @@
 pub mod gcode;
+use lazy_static::lazy_static;
 
 use bytes::BytesMut;
 use futures::sink::SinkExt;
@@ -13,7 +14,7 @@ use tokio_util::codec::{Decoder, Encoder, Framed};
 
 pub use gcode::{GcodeCommand, GcodeReply};
 
-use crate::printer::gcode::M114Command;
+use crate::printer::gcode::{G91Command, M114Command};
 
 use self::gcode::GcodeReplyError;
 
@@ -42,6 +43,7 @@ pub struct Printer {
     serial_tx: SplitSink<Framed<SerialStream, LineCodec>, String>,
     serial_rx: SplitStream<Framed<SerialStream, LineCodec>>,
     last_buffer_capacity: usize,
+    maximum_buffer_capacity: usize,
 }
 
 impl Printer {
@@ -88,7 +90,7 @@ impl Printer {
         // 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_secs(10), serial_rx.next()).await {
+            if let Ok(message) = timeout(Duration::from_secs(2), serial_rx.next()).await {
                 match message {
                     Some(Ok(reply)) => {
                         println!("got stuff: {:?}", reply);
@@ -111,7 +113,6 @@ impl Printer {
                 break;
             }
         }
-        println!("Sending M114 command");
 
         let mut res = Printer {
             serial_rx,
@@ -120,23 +121,40 @@ impl Printer {
                 position: (0.0, 0.0, 0.0, 0.0),
             }),
             last_buffer_capacity: 0, // this is updated on the next call to `send_gcode()`
+            maximum_buffer_capacity: 0, // this is updated on the next call to `send_gcode()`
         };
 
-        res.send_gcode(Box::new(M114Command))
+        res.send_gcode(Box::new(G91Command))
             .await
             .expect("Could not ask for current position!");
 
+        // since we never sent any positioning GCODE, we should be at max-capacity now.
+        res.maximum_buffer_capacity = res.last_buffer_capacity;
+
         Ok(res)
     }
 
+    /// The maximum capacity of the machines GCODE buffer.
+    pub fn maximum_capacity(&self) -> usize {
+        self.maximum_buffer_capacity
+    }
+
+    /// 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
+    }
+
     /// Parse the "Ok" confirmation line that the printer sends after every successfully received
     /// command.
     fn parse_ok(line: &str) -> Result<usize, PrinterError> {
         let make_err = || PrinterError::ConfirmationError {
             parsed_string: line.to_string(),
         };
-        let re = Regex::new(r"ok P(\d+) B(\d+)").unwrap();
-        let captures = re.captures(line).ok_or_else(make_err)?;
+        lazy_static! {
+            static ref RE: Regex = Regex::new(r"ok P(\d+) B(\d+)").unwrap();
+        }
+        let captures = RE.captures(line).ok_or_else(make_err)?;
         captures
             .get(1)
             .unwrap()