From d82eb8f22b5daa9405de65aebe2e4266cb2fe36f Mon Sep 17 00:00:00 2001
From: Sameer Puri <sameer@users.noreply.github.com>
Date: Fri, 26 Apr 2019 21:30:04 -0500
Subject: [PATCH] Reset initpos after reset(), adjust default settings for my
 pen plotter, output feedrate only when changed

---
 src/code.rs   | 23 +++++++++++++++++++++++
 src/main.rs   | 12 +++++++++---
 src/turtle.rs |  1 +
 3 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/src/code.rs b/src/code.rs
index d9f02d0..43deb86 100644
--- a/src/code.rs
+++ b/src/code.rs
@@ -59,6 +59,7 @@ pub enum GCode {
 
 pub fn program2gcode<W: Write>(p: &Program, mut w: W) -> io::Result<()> {
     use GCode::*;
+    let mut last_feedrate = None;
     for code in p.iter() {
         match code {
             RapidPositioning { x, y } => {
@@ -74,6 +75,28 @@ pub fn program2gcode<W: Write>(p: &Program, mut w: W) -> io::Result<()> {
                 if let (None, None, None, None) = (x, y, z, f) {
                     continue;
                 }
+
+                let f = match (last_feedrate, f) {
+                    (None, None) => {
+                        return Err(io::Error::new(
+                            io::ErrorKind::Other,
+                            "Linear interpolation without previously set feedrate",
+                        ))
+                    }
+                    (Some(last), Some(new)) => {
+                        if last != new {
+                            last_feedrate = Some(new);
+                            Some(new)
+                        } else {
+                            None
+                        }
+                    }
+                    (None, Some(new)) => {
+                        last_feedrate = Some(new);
+                        Some(new)
+                    }
+                    (Some(_), None) => None,
+                };
                 write!(w, "G1")?;
                 write_if_some!(w, " X{}", x)?;
                 write_if_some!(w, " Y{}", y)?;
diff --git a/src/main.rs b/src/main.rs
index b491abc..9944e29 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -66,16 +66,22 @@ fn main() -> io::Result<()> {
     }
 
     if true {
-        mach.tool_on_action = vec![GCode::StopSpindle, GCode::Dwell { p: 1.5 }];
+        mach.tool_on_action = vec![
+            GCode::StartSpindle {
+                d: Direction::Clockwise,
+                s: 70.0,
+            },
+            GCode::Dwell { p: 0.1 },
+        ];
     }
     if true {
         mach.tool_off_action = vec![
             GCode::Dwell { p: 0.1 },
             GCode::StartSpindle {
                 d: Direction::Clockwise,
-                s: 40.0,
+                s: 50.0,
             },
-            GCode::Dwell { p: 0.2 },
+            GCode::Dwell { p: 0.1 },
         ];
     }
 
diff --git a/src/turtle.rs b/src/turtle.rs
index 1be7b21..f49c9e8 100644
--- a/src/turtle.rs
+++ b/src/turtle.rs
@@ -386,5 +386,6 @@ impl Turtle {
         self.curpos = point(0.0, 0.0);
         self.curpos = self.curtran.transform_point(&self.curpos);
         self.prev_ctrl = self.curpos;
+        self.initpos = self.curpos;
     }
 }