From 797dd215d2172ebb81d205c32a29891f62535719 Mon Sep 17 00:00:00 2001
From: Frederik Menke <frederik.menke@rwth-aachen.de>
Date: Thu, 27 Oct 2022 00:02:06 +0200
Subject: [PATCH] Add G0

---
 red/src/main.rs              | 25 +++++++++++++++++++++++-
 red/src/printer/gcode/g0.rs  | 38 ++++++++++++++++++++++++++++++++++++
 red/src/printer/gcode/g28.rs |  2 +-
 red/src/printer/gcode/mod.rs |  4 +++-
 4 files changed, 66 insertions(+), 3 deletions(-)
 create mode 100644 red/src/printer/gcode/g0.rs

diff --git a/red/src/main.rs b/red/src/main.rs
index e06696a..4781e83 100644
--- a/red/src/main.rs
+++ b/red/src/main.rs
@@ -19,7 +19,30 @@ async fn main() -> tokio_serial::Result<()> {
         .await
         .unwrap();
     loop {
-        tokio::time::sleep(std::time::Duration::from_secs(2)).await;
+        tokio::time::sleep(std::time::Duration::from_secs(5)).await;
+        printer
+            .printer_in
+            .send(Box::new(G0Command {
+                x: Some(50.0),
+                y: Some(50.0),
+                z: Some(5.0),
+                e: None,
+                velocity: None,
+            }))
+            .await
+            .unwrap();
+        tokio::time::sleep(std::time::Duration::from_secs(5)).await;
+        printer
+            .printer_in
+            .send(Box::new(G0Command {
+                x: Some(25.0),
+                y: Some(25.0),
+                z: Some(5.0),
+                e: None,
+                velocity: None,
+            }))
+            .await
+            .unwrap();
     }
     Ok(())
 }
diff --git a/red/src/printer/gcode/g0.rs b/red/src/printer/gcode/g0.rs
new file mode 100644
index 0000000..335a7d2
--- /dev/null
+++ b/red/src/printer/gcode/g0.rs
@@ -0,0 +1,38 @@
+use super::*;
+
+#[derive(Debug)]
+pub struct G0Command {
+    pub x: Option<f64>,
+    pub y: Option<f64>,
+    pub z: Option<f64>,
+    pub e: Option<f64>,
+    pub velocity: Option<f64>,
+}
+
+impl GcodeCommand for G0Command {
+    fn command(&self) -> String {
+        fn unpack(letter: &str, o: Option<f64>) -> String {
+            o.map(|x| format!("{}{:.3}", letter, x)).unwrap_or_default()
+        }
+        format!(
+            "G0 {} {} {} {} {}",
+            unpack("X", self.x),
+            unpack("Y", self.y),
+            unpack("Z", self.z),
+            unpack("E", self.e),
+            unpack("F", self.velocity)
+        )
+    }
+
+    fn parse_reply(&self, reply: &str) -> Result<GcodeReply> {
+        if reply.is_empty() {
+            Ok(GcodeReply::NoReply)
+        } else {
+            Err(GcodeReplyError {
+                sent_command: self.command(),
+                parsed_input: reply.to_string(),
+                problem: "Expected empty reply".to_string(),
+            })
+        }
+    }
+}
diff --git a/red/src/printer/gcode/g28.rs b/red/src/printer/gcode/g28.rs
index a62e383..fbe480b 100644
--- a/red/src/printer/gcode/g28.rs
+++ b/red/src/printer/gcode/g28.rs
@@ -33,7 +33,7 @@ impl GcodeCommand for G28Command {
                 problem: "Expected no reply".to_string(),
             })
         } else {
-            Ok(GcodeReply::G28Reply)
+            Ok(GcodeReply::NoReply)
         }
     }
 }
diff --git a/red/src/printer/gcode/mod.rs b/red/src/printer/gcode/mod.rs
index 8442e04..b6879fd 100644
--- a/red/src/printer/gcode/mod.rs
+++ b/red/src/printer/gcode/mod.rs
@@ -1,5 +1,7 @@
+mod g0;
 mod g28;
 mod m114;
+pub use g0::G0Command;
 pub use g28::G28Command;
 pub use m114::M114Command;
 use std::fmt::Debug;
@@ -9,7 +11,7 @@ type Result<T> = std::result::Result<T, GcodeReplyError>;
 #[derive(Debug)]
 pub enum GcodeReply {
     M114Reply { x: f64, y: f64, z: f64, e: f64 },
-    G28Reply,
+    NoReply,
 }
 
 #[derive(Debug, Clone)]