From b47a27cb8651febe5bd8e5e0883853b599bfe451 Mon Sep 17 00:00:00 2001
From: Frederik Menke <frederik.menke@rwth-aachen.de>
Date: Mon, 1 Jan 2024 13:05:11 +0100
Subject: [PATCH] Jog using absolute movements

---
 red/src/jogger.rs      | 16 +++++++++++++---
 red/src/main.rs        | 12 ------------
 red/src/printer/mod.rs | 18 +++++++++++-------
 3 files changed, 24 insertions(+), 22 deletions(-)

diff --git a/red/src/jogger.rs b/red/src/jogger.rs
index ccc4dc0..5902b69 100644
--- a/red/src/jogger.rs
+++ b/red/src/jogger.rs
@@ -33,8 +33,8 @@ pub type PrinterVec = Vector3D<f64, PrinterUnits>;
 
 /// Jog the gantry by pumping loads of gcode into the printer board
 pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
-    printer.use_relative_movements().await.unwrap();
-    println!("Using relative movements");
+    printer.use_absolute_movements().await.unwrap();
+    println!("Using absolute movements");
     loop {
         let (setpoint_x, setpoint_y, setpoint_z) = gamepad.speed_setpoint();
 
@@ -48,11 +48,21 @@ pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
             continue;
         }
         let velocity = distance.length() / (TIME_PER_MOVEMENT.as_secs_f64() / 60.0);
+        let old_postion = printer.state.position;
         printer
-            .move_relative(distance.x, distance.y, distance.z, velocity.into())
+            .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!");
 
+        println!(
+            "New position {pos:?}",
+            pos = printer.printer_state().position
+        );
         // 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();
diff --git a/red/src/main.rs b/red/src/main.rs
index cccd054..a1c5b88 100644
--- a/red/src/main.rs
+++ b/red/src/main.rs
@@ -81,15 +81,3 @@ async fn write_to_printer() -> Never {
         printer.move_relative(25.0, 25.0, 5.0, None).await.unwrap();
     }
 }
-
-async fn jog() -> Never {
-    let jogger = gamepad::Gamepad::new().await.unwrap();
-    loop {
-        tokio::time::sleep(Duration::from_secs(2)).await;
-        let setpoint = jogger.speed_setpoint();
-        println!(
-            "speed setpoint: {} {} {}",
-            setpoint.0, setpoint.1, setpoint.2
-        );
-    }
-}
diff --git a/red/src/printer/mod.rs b/red/src/printer/mod.rs
index c3d2901..73f8421 100644
--- a/red/src/printer/mod.rs
+++ b/red/src/printer/mod.rs
@@ -39,9 +39,9 @@ pub enum PrinterError {
 
 #[derive(Debug, Clone, Copy)]
 pub struct PrinterPosition {
-    x: f64,
-    y: f64,
-    z: f64,
+    pub x: f64,
+    pub y: f64,
+    pub z: f64,
 }
 
 pub enum MovementMode {
@@ -50,8 +50,8 @@ pub enum MovementMode {
 }
 
 pub struct State {
-    position: PrinterPosition,
-    movement_mode: MovementMode,
+    pub position: PrinterPosition,
+    pub movement_mode: MovementMode,
 }
 
 pub struct Printer {
@@ -163,6 +163,10 @@ impl Printer {
         Ok(res)
     }
 
+    pub fn printer_state(&self) -> &State {
+        &self.state
+    }
+
     /// The maximum capacity of the machines GCODE buffer.
     pub fn maximum_capacity(&self) -> usize {
         self.maximum_buffer_capacity
@@ -289,14 +293,14 @@ impl Printer {
         x: f64,
         y: f64,
         z: f64,
-        velocity: f64,
+        velocity: Option<f64>,
     ) -> Result<(), PrinterError> {
         let command = G0Command {
             x: Some(x),
             y: Some(y),
             z: Some(z),
             e: None, // Machine has no e
-            velocity: Some(velocity),
+            velocity: velocity,
         };
         if let MovementMode::RelativeMovements = self.state.movement_mode {
             self.use_absolute_movements().await?;