From af069c8e5e97c8923be1d6051b122d5c403bc5da Mon Sep 17 00:00:00 2001
From: Frederik Menke <frederik.menke@rwth-aachen.de>
Date: Sat, 18 Nov 2023 22:33:53 +0100
Subject: [PATCH] Implement first test

---
 red/src/jogger.rs | 45 +++++++++++++++++++++++++++++++++++++++++++++
 red/src/lib.rs    |  1 +
 red/src/main.rs   |  7 ++++++-
 3 files changed, 52 insertions(+), 1 deletion(-)
 create mode 100644 red/src/jogger.rs

diff --git a/red/src/jogger.rs b/red/src/jogger.rs
new file mode 100644
index 0000000..d9406f9
--- /dev/null
+++ b/red/src/jogger.rs
@@ -0,0 +1,45 @@
+use crate::printer::GcodeCommand;
+use crate::{gamepad::Gamepad, printer::gcode::G0Command, printer::Printer};
+use euclid::{vec3, Vector3D};
+use futures::never::Never;
+use std::sync::Arc;
+use std::time::Duration;
+
+/// Time that a single movement command should take
+///
+/// For a given GCode buffer size on the machine, we can assume
+/// a maximum delay of `TIME_PER_MOVEMENT * BUFFER_COUNT`
+const TIME_PER_MOVEMENT: Duration = Duration::from_millis(20);
+/// Movement speed of gantry when going full throttle in x/y-direction
+/// in units/min (mm/min)
+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;
+
+/// 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
+pub async fn jog(gamepad: Arc<Gamepad>, mut printer: Printer) -> Never {
+    loop {
+        let (setpoint_x, setpoint_y, setpoint_z) = gamepad.speed_setpoint();
+        let distance: PrinterVec = vec3(
+            (FULL_SCALE_SPEED_XY / 60.0) * TIME_PER_MOVEMENT.as_secs_f64() * (setpoint_x as f64),
+            (FULL_SCALE_SPEED_XY / 60.0) * TIME_PER_MOVEMENT.as_secs_f64() * (setpoint_y as f64),
+            (FULL_SCALE_SPEED_Z / 60.0) * TIME_PER_MOVEMENT.as_secs_f64() * (setpoint_z as f64),
+        );
+        let velocity = distance.length();
+        let command = G0Command {
+            x: distance.x.into(),
+            y: distance.y.into(),
+            z: distance.z.into(),
+            e: None,
+            velocity: velocity.into(),
+        };
+        // printer.send_gcode(Box::new(command)).await;
+        println!("{:?}", command.command());
+        std::thread::sleep(TIME_PER_MOVEMENT);
+    }
+}
diff --git a/red/src/lib.rs b/red/src/lib.rs
index 2e00e3b..b3df052 100644
--- a/red/src/lib.rs
+++ b/red/src/lib.rs
@@ -1,2 +1,3 @@
 pub mod gamepad;
+pub mod jogger;
 pub mod printer;
diff --git a/red/src/main.rs b/red/src/main.rs
index 2777cba..265c547 100644
--- a/red/src/main.rs
+++ b/red/src/main.rs
@@ -2,6 +2,7 @@
 use futures::never::Never;
 use i2c_linux::I2c;
 use red::gamepad;
+use red::jogger;
 use red::printer::gcode::*;
 use red::printer::Printer;
 use std::time::Duration;
@@ -33,7 +34,11 @@ const I2C_REGISTER_SPINDLE_SPEED: u8 = 0;
 
 #[tokio::main]
 async fn main() -> Never {
-    jog().await
+    print!("Entering App");
+    let gamepad = gamepad::Gamepad::new().await.unwrap();
+    let printer = Printer::connect(DEFAULT_TTY).await.unwrap();
+    // TODO: Make printer do relative movements
+    jogger::jog(gamepad, printer).await
 }
 
 async fn write_to_spindle() -> Never {