From 7c041f2f8f42ad94c70cf1a2fdbc6cd2867b76de Mon Sep 17 00:00:00 2001
From: IhorNehrutsa <Ihor.Nehrutsa@gmail.com>
Date: Tue, 3 Aug 2021 22:20:38 +0300
Subject: [PATCH 01/11] Add G0, G90, G91

---
 src/config/config.h      |  2 +-
 src/config/config_adv.h  |  2 +-
 src/hardware/motor.h     |  6 ++++++
 src/software/GM_code.cpp |  6 ++++++
 src/software/GM_code.h   | 39 +++++++++++++++++++++++++++++++++++++++
 src/software/parser.cpp  | 38 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 91 insertions(+), 2 deletions(-)
 create mode 100644 src/software/GM_code.cpp
 create mode 100644 src/software/GM_code.h

diff --git a/src/config/config.h b/src/config/config.h
index f74ba2d2..39937c29 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -115,7 +115,7 @@
 
 // PID settings
 // ! At this time, this feature is still under development
-#define ENABLE_PID
+//#define ENABLE_PID
 #ifdef ENABLE_PID
 
     // Default P, I, and D terms
diff --git a/src/config/config_adv.h b/src/config/config_adv.h
index 762fba2a..06fc1cde 100644
--- a/src/config/config_adv.h
+++ b/src/config/config_adv.h
@@ -116,7 +116,7 @@ typedef float real_t;
 
 //#define ENABLE_STEPPING_VELOCITY
 //#define IGNORE_FLASH_VERSION
-//#define DISABLE_CORRECTION_TIMER
+#define DISABLE_CORRECTION_TIMER
 
 // LED related debugging
 #ifdef ENABLE_LED
diff --git a/src/hardware/motor.h b/src/hardware/motor.h
index 177d253a..9b47d272 100644
--- a/src/hardware/motor.h
+++ b/src/hardware/motor.h
@@ -6,6 +6,7 @@
 #include "HardwareTimer.h"
 #include "encoder.h"
 #include "fastAnalogWrite.h"
+#include "GM_code.h"
 #include "stm32f1xx_hal_tim.h"
 
 // For sin() and fmod() function
@@ -224,6 +225,11 @@ class StepperMotor {
         // Counter for number of overflows (needs to be public for the interrupt)
         int32_t stepOverflowOffset = 0;
 
+        // GM_code state machine
+        GM_code gm_code;
+
+        // Motor axis
+        AXES axis = A_AXIS;
 
     // Things that shouldn't be accessed by the outside
     private:
diff --git a/src/software/GM_code.cpp b/src/software/GM_code.cpp
new file mode 100644
index 00000000..589b9cb8
--- /dev/null
+++ b/src/software/GM_code.cpp
@@ -0,0 +1,6 @@
+#include "GM_code.h"
+
+// Main constructor
+GM_code::GM_code() {
+
+}
diff --git a/src/software/GM_code.h b/src/software/GM_code.h
new file mode 100644
index 00000000..ccdd7f2f
--- /dev/null
+++ b/src/software/GM_code.h
@@ -0,0 +1,39 @@
+#ifndef _GM_code_H__
+#define _GM_code_H__
+
+// Enumeration for G code distance mode
+typedef enum {
+    ABSOLUTE = 90,
+    INCREMENTAL = 91
+} DISTANCE_MODE;
+
+typedef enum {
+    X_AXIS = 'X', // main linear axes
+    Y_AXIS = 'Y',
+    Z_AXIS = 'Z',
+
+    A_AXIS = 'A', // rotary axes
+    B_AXIS = 'B',
+    C_AXIS = 'C',
+
+    U_AXIS = 'U', // additional axes
+    V_AXIS = 'V',
+    W_AXIS = 'W'
+} AXES;
+
+// GM code class stores a variables
+class GM_code {
+
+    public:
+
+        // Initialize
+        GM_code();
+
+        // Keeps the current G code distance_mode
+        DISTANCE_MODE distance_mode = ABSOLUTE;
+
+    private:
+
+};
+
+#endif
diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index 97301ad8..e7c966d6 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -5,11 +5,16 @@
 #if defined(ENABLE_SERIAL) || defined(ENABLE_CAN)
 
 #include "parser.h"
+#include "GM_code.h"
 
 // Parses an entire string for any commands
 String parseCommand(String buffer) {
 
     // Gcode Table
+    //  - G90 Absolute positioning // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g90-g91
+    //  - G91 incremental positioning // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g90-g91
+    //  - G0 (ex G0 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
+    //  - G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move. Requires `ENABLE_DIRECT_STEPPING`
     //  - M17 (ex M17) - Enables the motor (overrides enable pin)
     //  - M18 / M84 (ex M18 or M84) - Disables the motor (overrides enable pin)
     //  - M93 (ex M93 V1.8 or M93) - Sets the angle of a full step. This value should be 1.8° or 0.9°. If no value is provided, then the current value will be returned.
@@ -419,6 +424,39 @@ String parseCommand(String buffer) {
                 return FEEDBACK_OK;
             }
 
+            case 0: {
+                //  - G0 (ex G0 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
+                // Pull the values from the command
+                float value = parseValue(buffer, (char)motor.axis).toFloat();
+
+                int32_t rate = DEFAULT_STEPPING_RATE;
+                int32_t count = value / motor.getMicrostepAngle();
+
+                if (motor.gm_code.distance_mode == ABSOLUTE) {
+                    count -= motor.getDesiredStep();
+                }
+
+                STEP_DIR step_dir = COUNTER_CLOCKWISE;
+                if (count < 0.0) {
+                    count = abs(count);
+                    step_dir = CLOCKWISE;
+                }
+
+                scheduleSteps(count, rate, step_dir);
+                return FEEDBACK_OK;
+            }
+
+
+            case 90: {
+                motor.gm_code.distance_mode = ABSOLUTE;
+                return FEEDBACK_OK;
+            }
+
+            case 91: {
+                motor.gm_code.distance_mode = INCREMENTAL;
+                return FEEDBACK_OK;
+            }
+
             default: {
                 // Command isn't recognized, therefore throw an error
                 return FEEDBACK_CMD_NOT_AVAILABLE;

From 711bbc34ee34c3591139ed30c6bbca1ea7f5cc35 Mon Sep 17 00:00:00 2001
From: IhorNehrutsa <Ihor.Nehrutsa@gmail.com>
Date: Wed, 4 Aug 2021 22:18:55 +0300
Subject: [PATCH 02/11] Save a current G code rate for G6, G0

---
 src/software/GM_code.h  |  5 +++++
 src/software/parser.cpp | 15 +++++++++++++--
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/src/software/GM_code.h b/src/software/GM_code.h
index ccdd7f2f..8c96a5c2 100644
--- a/src/software/GM_code.h
+++ b/src/software/GM_code.h
@@ -1,6 +1,8 @@
 #ifndef _GM_code_H__
 #define _GM_code_H__
 
+#include "config.h"
+
 // Enumeration for G code distance mode
 typedef enum {
     ABSOLUTE = 90,
@@ -32,6 +34,9 @@ class GM_code {
         // Keeps the current G code distance_mode
         DISTANCE_MODE distance_mode = ABSOLUTE;
 
+        // Keeps the current G code rate for G6, G0 in Hz
+        int32_t rate = DEFAULT_STEPPING_RATE;
+
     private:
 
 };
diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index e7c966d6..ad764d4e 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -406,7 +406,10 @@ String parseCommand(String buffer) {
 
                 // Sanitize the inputs
                 if (rate <= 0) {
-                    rate = DEFAULT_STEPPING_RATE;
+                    rate = motor.gm_code.rate;
+                }
+                else {
+                    motor.gm_code.rate = rate;
                 }
                 if (count <= 0) {
                     return FEEDBACK_NO_VALUE;
@@ -428,8 +431,16 @@ String parseCommand(String buffer) {
                 //  - G0 (ex G0 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
                 // Pull the values from the command
                 float value = parseValue(buffer, (char)motor.axis).toFloat();
+                int32_t rate = parseValue(buffer, 'R').toInt();
+
+                // Sanitize the inputs
+                if (rate <= 0) {
+                    rate = motor.gm_code.rate;
+                }
+                else {
+                    motor.gm_code.rate = rate;
+                }
 
-                int32_t rate = DEFAULT_STEPPING_RATE;
                 int32_t count = value / motor.getMicrostepAngle();
 
                 if (motor.gm_code.distance_mode == ABSOLUTE) {

From c404ea8cb1628df4df8f7e1d2c914354aac8675c Mon Sep 17 00:00:00 2001
From: IhorNehrutsa <Ihor.Nehrutsa@gmail.com>
Date: Wed, 4 Aug 2021 22:35:17 +0300
Subject: [PATCH 03/11] Update GM_code.h

---
 src/software/GM_code.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/software/GM_code.h b/src/software/GM_code.h
index 8c96a5c2..8444ddb2 100644
--- a/src/software/GM_code.h
+++ b/src/software/GM_code.h
@@ -34,8 +34,10 @@ class GM_code {
         // Keeps the current G code distance_mode
         DISTANCE_MODE distance_mode = ABSOLUTE;
 
+        #ifdef ENABLE_DIRECT_STEPPING
         // Keeps the current G code rate for G6, G0 in Hz
         int32_t rate = DEFAULT_STEPPING_RATE;
+        #endif
 
     private:
 

From c0694231b1911462259645bc1768d487b0a3a13c Mon Sep 17 00:00:00 2001
From: IhorNehrutsa <Ihor.Nehrutsa@gmail.com>
Date: Thu, 5 Aug 2021 22:50:22 +0300
Subject: [PATCH 04/11] Update parser.cpp

---
 src/software/parser.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index ad764d4e..b8cad506 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -14,6 +14,7 @@ String parseCommand(String buffer) {
     //  - G90 Absolute positioning // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g90-g91
     //  - G91 incremental positioning // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g90-g91
     //  - G0 (ex G0 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
+    //    https://marlinfw.org/docs/gcode/G006.html
     //  - G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move. Requires `ENABLE_DIRECT_STEPPING`
     //  - M17 (ex M17) - Enables the motor (overrides enable pin)
     //  - M18 / M84 (ex M18 or M84) - Disables the motor (overrides enable pin)

From 4b20cfdf320d2f96b1e1abb63eb084dcfe658500 Mon Sep 17 00:00:00 2001
From: Christian Piper <42127153+CAP1Sup@users.noreply.github.com>
Date: Thu, 5 Aug 2021 18:39:47 -0400
Subject: [PATCH 05/11] Added steps/mm, fixed some issues, renamed gm_codes to
 planner

Added FULL_MOTION_PLANNER support
---
 README.md                   |  3 ++
 buildroot/tests/BTT_S42B_V2 | 15 ++++--
 src/config/config.h         | 18 ++++---
 src/hardware/motor.cpp      | 18 +++++++
 src/hardware/motor.h        | 24 ++++++++--
 src/software/GM_code.cpp    |  6 ---
 src/software/GM_code.h      | 46 ------------------
 src/software/parser.cpp     | 94 +++++++++++++++++++++++--------------
 src/software/parser.h       | 13 ++---
 src/software/planner.cpp    | 46 ++++++++++++++++++
 src/software/planner.h      | 77 ++++++++++++++++++++++++++++++
 src/software/sanityCheck.h  |  6 +++
 12 files changed, 258 insertions(+), 108 deletions(-)
 delete mode 100644 src/software/GM_code.cpp
 delete mode 100644 src/software/GM_code.h
 create mode 100644 src/software/planner.cpp
 create mode 100644 src/software/planner.h

diff --git a/README.md b/README.md
index e7d5f6ea..390c743e 100644
--- a/README.md
+++ b/README.md
@@ -27,7 +27,10 @@ Future Features:
 
 G/M Code Table
 
+- G0 (ex G0 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units. Requires `ENABLE_FULL_MOTION_PLANNER`
 - G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move. Requires `ENABLE_DIRECT_STEPPING`
+- G90 (ex G90) - Sets the movement units in absolute positioning. Requires `ENABLE_FULL_MOTION_PLANNER`
+- G91 (ex G91) - Sets the movement units in relative (incremental) positioning. Requires `ENABLE_FULL_MOTION_PLANNER`
 - M17 (ex M17) - Enables the motor (overrides enable pin)
 - M18 / M84 (ex M18 or M84) - Disables the motor (overrides enable pin)
 - M93 (ex M93 V1.8 or M93) - Sets the angle of a full step. This value should be 1.8° or 0.9°. If no value is provided, then the current value will be returned.
diff --git a/buildroot/tests/BTT_S42B_V2 b/buildroot/tests/BTT_S42B_V2
index 4478ea02..db14ef8b 100755
--- a/buildroot/tests/BTT_S42B_V2
+++ b/buildroot/tests/BTT_S42B_V2
@@ -10,14 +10,19 @@ set -e
 # Build with the default configurations
 #
 restore_configs
-opt_enable ENABLE_OLED ENABLE_CAN ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_DYNAMIC_CURRENT ENABLE_PID ENABLE_DIRECT_STEPPING
-exec_test $1 $2 "OLED, CAN, Serial, StallFault, Dynamic Current, PID, Direct Stepping" "$3"
+opt_enable ENABLE_OLED ENABLE_CAN ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_DYNAMIC_CURRENT ENABLE_PID ENABLE_FULL_MOTION_PLANNER ENABLE_DIRECT_STEPPING
+exec_test $1 $2 "OLED, CAN, Serial, StallFault, Dynamic Current, PID, Motion Planner, Direct Stepping" "$3"
 
 restore_configs
-opt_enable ENABLE_OLED ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_OVERTEMP_PROTECTION ENABLE_PID ENABLE_DIRECT_STEPPING
+opt_enable ENABLE_OLED ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_OVERTEMP_PROTECTION ENABLE_PID ENABLE_FULL_MOTION_PLANNER ENABLE_DIRECT_STEPPING
 opt_disable ENABLE_CAN ENABLE_DYNAMIC_CURRENT
-exec_test $1 $2 "OLED, Serial, Stallfault, Overtemp, PID, Direct Stepping" "$3"
+exec_test $1 $2 "OLED, Serial, Stallfault, Overtemp, PID, Motion Planner, Direct Stepping" "$3"
 
 restore_configs
-opt_disable ENABLE_OLED ENABLE_CAN ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_DYNAMIC_CURRENT ENABLE_OVERTEMP_PROTECTION ENABLE_PID ENABLE_DIRECT_STEPPING
+opt_enable ENABLE_OLED ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_OVERTEMP_PROTECTION ENABLE_PID ENABLE_FULL_MOTION_PLANNER
+opt_disable ENABLE_CAN ENABLE_DYNAMIC_CURRENT ENABLE_DIRECT_STEPPING
+exec_test $1 $2 "OLED, Serial, Stallfault, Overtemp, PID, Motion Planner" "$3"
+
+restore_configs
+opt_disable ENABLE_OLED ENABLE_CAN ENABLE_SERIAL ENABLE_STALLFAULT ENABLE_DYNAMIC_CURRENT ENABLE_OVERTEMP_PROTECTION ENABLE_PID ENABLE_FULL_MOTION_PLANNER ENABLE_DIRECT_STEPPING
 exec_test $1 $2 "No extra options" "$3"
\ No newline at end of file
diff --git a/src/config/config.h b/src/config/config.h
index 39937c29..4c984a68 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -136,13 +136,17 @@
     #define DEFAULT_PID_DISABLE_THRESHOLD 0 //1000
 #endif
 
-// Direct step functionality (used to command motor to move over Serial/CAN)
-#define ENABLE_DIRECT_STEPPING
-#ifdef ENABLE_DIRECT_STEPPING
-
-    // The default stepping rate (in Hz) to move in the event that no parameter is specified
-    #define DEFAULT_STEPPING_RATE 1000
-#endif
+// Full motion planner (allows for G code control of motor)
+#define ENABLE_FULL_MOTION_PLANNER
+#ifdef ENABLE_FULL_MOTION_PLANNER
+    // Direct step functionality (used to command motor to move over Serial/CAN)
+    #define ENABLE_DIRECT_STEPPING
+    #ifdef ENABLE_DIRECT_STEPPING
+
+        // The default stepping rate (in Hz) to move in the event that no parameter is specified
+        #define DEFAULT_STEPPING_RATE 1000
+    #endif
+#endif // ! ENABLE_FULL_MOTION_PLANNER
 
 // Motor settings
 // The number of microsteps to move per step pulse
diff --git a/src/hardware/motor.cpp b/src/hardware/motor.cpp
index 4ed60bc4..8403e01f 100644
--- a/src/hardware/motor.cpp
+++ b/src/hardware/motor.cpp
@@ -353,6 +353,11 @@ void StepperMotor::setMicrostepping(uint16_t setMicrostepping, bool lock) {
             setSoftStepCNT(getSoftStepCNT() * stepScalingFactor);
         #endif
 
+        // Scale the steps per mm (only used if FULL_MOTION_PLANNER is enabled)
+        #ifdef ENABLE_FULL_MOTION_PLANNER
+        this -> stepsPerMM = round(stepsPerMM * stepScalingFactor);
+        #endif
+
         // Scale the microstep multiplier so that the full stepping level is maintained
         // This needs to be done before the new divisor is set
         #ifdef MAINTAIN_FULL_STEPPING
@@ -414,6 +419,19 @@ int32_t StepperMotor::getMicrostepsPerRotation() const {
     return (this -> microstepsPerRotation);
 }
 
+// Only needed if FULL_MOTION_PLANNER is enabled
+#ifdef ENABLE_FULL_MOTION_PLANNER
+// Set the steps per mm of the motor
+void StepperMotor::setStepsPerMM(uint16_t newStepsPerMM) {
+    this -> stepsPerMM = newStepsPerMM;
+}
+
+
+// Get the steps per mm of the motor
+uint16_t StepperMotor::getStepsPerMM() {
+    return (this -> stepsPerMM);
+}
+#endif // ! ENABLE_FULL_MOTION_PLANNER
 
 // Set if the motor direction should be reversed or not
 void StepperMotor::setReversed(bool reversed) {
diff --git a/src/hardware/motor.h b/src/hardware/motor.h
index 9b47d272..100bb5d8 100644
--- a/src/hardware/motor.h
+++ b/src/hardware/motor.h
@@ -6,7 +6,7 @@
 #include "HardwareTimer.h"
 #include "encoder.h"
 #include "fastAnalogWrite.h"
-#include "GM_code.h"
+#include "planner.h"
 #include "stm32f1xx_hal_tim.h"
 
 // For sin() and fmod() function
@@ -164,6 +164,15 @@ class StepperMotor {
         // Get the microsteps per rotation of the motor
         int32_t getMicrostepsPerRotation() const;
 
+        // Only needed if FULL_MOTION_PLANNER is enabled
+        #ifdef ENABLE_FULL_MOTION_PLANNER
+        // Set the steps per mm of the motor
+        void setStepsPerMM(uint16_t newStepsPerMM);
+
+        // Get the steps per mm of the motor
+        uint16_t getStepsPerMM();
+        #endif // ! ENABLE_FULL_MOTION_PLANNER
+
         // Set if the motor should be reversed
         void setReversed(bool reversed);
 
@@ -225,11 +234,15 @@ class StepperMotor {
         // Counter for number of overflows (needs to be public for the interrupt)
         int32_t stepOverflowOffset = 0;
 
-        // GM_code state machine
-        GM_code gm_code;
+        // Planner (used for motion support)
+        #ifdef ENABLE_FULL_MOTION_PLANNER
+        Planner planner;
+        #endif
 
         // Motor axis
+        #ifdef ENABLE_FULL_MOTION_PLANNER
         AXES axis = A_AXIS;
+        #endif
 
     // Things that shouldn't be accessed by the outside
     private:
@@ -286,6 +299,11 @@ class StepperMotor {
         // Microstep count in a full rotation
         int32_t microstepsPerRotation = (360.0 / getMicrostepAngle());
 
+        // Variable to save the steps per mm of the motor (only needed if FULL_MOTION_PLANNER is enabled)
+        #ifdef ENABLE_FULL_MOTION_PLANNER
+        uint16_t stepsPerMM = 0;
+        #endif
+
         // If the motor is enabled or not (saves time so that the enable and disable pins are only set once)
         MOTOR_STATE state = MOTOR_NOT_SET;
 
diff --git a/src/software/GM_code.cpp b/src/software/GM_code.cpp
deleted file mode 100644
index 589b9cb8..00000000
--- a/src/software/GM_code.cpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "GM_code.h"
-
-// Main constructor
-GM_code::GM_code() {
-
-}
diff --git a/src/software/GM_code.h b/src/software/GM_code.h
deleted file mode 100644
index 8444ddb2..00000000
--- a/src/software/GM_code.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef _GM_code_H__
-#define _GM_code_H__
-
-#include "config.h"
-
-// Enumeration for G code distance mode
-typedef enum {
-    ABSOLUTE = 90,
-    INCREMENTAL = 91
-} DISTANCE_MODE;
-
-typedef enum {
-    X_AXIS = 'X', // main linear axes
-    Y_AXIS = 'Y',
-    Z_AXIS = 'Z',
-
-    A_AXIS = 'A', // rotary axes
-    B_AXIS = 'B',
-    C_AXIS = 'C',
-
-    U_AXIS = 'U', // additional axes
-    V_AXIS = 'V',
-    W_AXIS = 'W'
-} AXES;
-
-// GM code class stores a variables
-class GM_code {
-
-    public:
-
-        // Initialize
-        GM_code();
-
-        // Keeps the current G code distance_mode
-        DISTANCE_MODE distance_mode = ABSOLUTE;
-
-        #ifdef ENABLE_DIRECT_STEPPING
-        // Keeps the current G code rate for G6, G0 in Hz
-        int32_t rate = DEFAULT_STEPPING_RATE;
-        #endif
-
-    private:
-
-};
-
-#endif
diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index b8cad506..2cb7e398 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -5,7 +5,6 @@
 #if defined(ENABLE_SERIAL) || defined(ENABLE_CAN)
 
 #include "parser.h"
-#include "GM_code.h"
 
 // Parses an entire string for any commands
 String parseCommand(String buffer) {
@@ -398,74 +397,99 @@ String parseCommand(String buffer) {
         // Switch statement the command number
         switch (parseValue(buffer, 'G').toInt()) {
 
-            case 6: {
-                // G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move
+            case 0: {
+                // - G0 (ex G0 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units
+                // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
                 // Pull the values from the command
-                bool reverse = parseValue(buffer, 'D').equals("1");
+                float value = parseValue(buffer, (char)motor.axis).toFloat();
                 int32_t rate = parseValue(buffer, 'R').toInt();
-                int64_t count = parseValue(buffer, 'S').toInt();
 
                 // Sanitize the inputs
                 if (rate <= 0) {
-                    rate = motor.gm_code.rate;
+                    rate = motor.planner.getLastStepRate();
                 }
-                else {
-                    motor.gm_code.rate = rate;
+
+                // Save the new rate for next time (needed if the next command doesn't specify rate)
+                motor.planner.setLastStepRate(rate);
+
+                // Determine the number of steps we need to move
+                // Create a count value for number of steps
+                int32_t count;
+
+                // Decide if units are mm or deg based on axis letter
+                if ((char)motor.axis == 'A' || (char)motor.axis == 'B' || (char)motor.axis == 'C') {
+
+                    // Units are degrees
+                    count = round(value / motor.getMicrostepAngle());
                 }
-                if (count <= 0) {
-                    return FEEDBACK_NO_VALUE;
+                else {
+                    // Units must be in mm
+                    // Check to make sure that steps per mm has been set
+                    if (motor.getStepsPerMM() > 0) {
+                        count = round(value * motor.getStepsPerMM());
+                    }
+                    else {
+                        // There is no steps per mm, throw an error
+                        return FEEDBACK_STEPS_PER_MM_NOT_SET;
+                    }
                 }
 
-                // Call the steps to be scheduled
-                if (!reverse) {
-                    scheduleSteps(count, rate, COUNTER_CLOCKWISE);
+                // Adjust the count by the current position if the mode is absolute
+                if (motor.planner.getDistanceMode() == ABSOLUTE) {
+                    count -= motor.getDesiredStep();
                 }
-                else {
-                    scheduleSteps(count, rate, CLOCKWISE);
+
+                // Default to CCW rotation
+                STEP_DIR dir = COUNTER_CLOCKWISE;
+
+                // If the count is negative (motor needs to move in opposite direction)
+                // then we can make the count positive and fix the direction
+                if (count < 0) {
+                    count = -count;
+                    dir = CLOCKWISE;
                 }
 
-                // All good, we can exit
+                // Schedule the steps to be moved
+                scheduleSteps(count, rate, dir);
+
+                // Return that everything went well
                 return FEEDBACK_OK;
             }
 
-            case 0: {
-                //  - G0 (ex G0 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
+            case 6: {
+                // G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move
                 // Pull the values from the command
-                float value = parseValue(buffer, (char)motor.axis).toFloat();
+                bool reverse = parseValue(buffer, 'D').equals("1");
                 int32_t rate = parseValue(buffer, 'R').toInt();
+                int64_t count = parseValue(buffer, 'S').toInt();
 
                 // Sanitize the inputs
                 if (rate <= 0) {
-                    rate = motor.gm_code.rate;
+                    rate = motor.planner.getDefaultSteppingRate();
                 }
-                else {
-                    motor.gm_code.rate = rate;
+                if (count <= 0) {
+                    return FEEDBACK_NO_VALUE;
                 }
 
-                int32_t count = value / motor.getMicrostepAngle();
-
-                if (motor.gm_code.distance_mode == ABSOLUTE) {
-                    count -= motor.getDesiredStep();
+                // Call the steps to be scheduled
+                if (!reverse) {
+                    scheduleSteps(count, rate, COUNTER_CLOCKWISE);
                 }
-
-                STEP_DIR step_dir = COUNTER_CLOCKWISE;
-                if (count < 0.0) {
-                    count = abs(count);
-                    step_dir = CLOCKWISE;
+                else {
+                    scheduleSteps(count, rate, CLOCKWISE);
                 }
 
-                scheduleSteps(count, rate, step_dir);
+                // All good, we can exit
                 return FEEDBACK_OK;
             }
 
-
             case 90: {
-                motor.gm_code.distance_mode = ABSOLUTE;
+                motor.planner.setDistanceMode(ABSOLUTE);
                 return FEEDBACK_OK;
             }
 
             case 91: {
-                motor.gm_code.distance_mode = INCREMENTAL;
+                motor.planner.setDistanceMode(INCREMENTAL);
                 return FEEDBACK_OK;
             }
 
diff --git a/src/software/parser.h b/src/software/parser.h
index 9714f6aa..673c3d48 100644
--- a/src/software/parser.h
+++ b/src/software/parser.h
@@ -7,12 +7,13 @@
 #include "config.h"
 
 // Defines for strings that are used repeatedly
-#define FEEDBACK_NO_VALUE          F("No value specified! Make sure to specify a value with a letter before it")
-#define FEEDBACK_OK                F("ok")
-#define FEEDBACK_CAN_NOT_ENABLED   F("CAN functionality not enabled")
-#define FEEDBACK_INVALID_STRING    F("Invalid string. Make sure that the string had double quotations on each side")
-#define FEEDBACK_NO_CMD_SPECIFIED  F("No command specified")
-#define FEEDBACK_CMD_NOT_AVAILABLE F("Command number not recognized")
+#define FEEDBACK_NO_VALUE             F("No value specified! Make sure to specify a value with a letter before it")
+#define FEEDBACK_OK                   F("ok")
+#define FEEDBACK_CAN_NOT_ENABLED      F("CAN functionality not enabled")
+#define FEEDBACK_INVALID_STRING       F("Invalid string. Make sure that the string had double quotations on each side")
+#define FEEDBACK_NO_CMD_SPECIFIED     F("No command specified")
+#define FEEDBACK_CMD_NOT_AVAILABLE    F("Command number not recognized")
+#define FEEDBACK_STEPS_PER_MM_NOT_SET F("Steps per mm not set")
 
 // Parse a string for commands, returning the feedback on the command
 String parseCommand(String buffer);
diff --git a/src/software/planner.cpp b/src/software/planner.cpp
new file mode 100644
index 00000000..1948868d
--- /dev/null
+++ b/src/software/planner.cpp
@@ -0,0 +1,46 @@
+// Include the main header file
+#include "planner.h"
+
+// Only include if FULL_MOTION_PLANNER is enabled
+#ifdef ENABLE_FULL_MOTION_PLANNER
+
+// Main constructor
+Planner::Planner() {}
+
+// Set distance mode
+void Planner::setDistanceMode(DISTANCE_MODE newDisMode) {
+    this -> distanceMode = newDisMode;
+}
+
+
+// Get the distance mode
+DISTANCE_MODE Planner::getDistanceMode() {
+    return (this -> distanceMode);
+}
+
+
+// Default rates are only needed with DIRECT_STEPPING
+#ifdef ENABLE_DIRECT_STEPPING
+// Set the default stepping rate for G6
+void Planner::setDefaultSteppingRate(int32_t newRate) {
+    this -> defaultRate = newRate;
+}
+
+
+// Get the default stepping rate for G6
+int32_t Planner::getDefaultSteppingRate() {
+    return (this -> defaultRate);
+}
+#endif // ! ENABLE_DIRECT_STEPPING
+
+// Set the last step rate
+void Planner::setLastStepRate(int32_t rate) {
+    this -> lastRate = rate;
+}
+
+// Get the last step rate
+int32_t Planner::getLastStepRate() {
+    return (this -> lastRate);
+}
+
+#endif // ! ENABLE_FULL_MOTION_PLANNER
\ No newline at end of file
diff --git a/src/software/planner.h b/src/software/planner.h
new file mode 100644
index 00000000..5de0def6
--- /dev/null
+++ b/src/software/planner.h
@@ -0,0 +1,77 @@
+#ifndef _PLANNER_H__
+#define _PLANNER_H__
+
+// Include the config
+#include "config.h"
+
+// Only include if the full motion planner is enabled
+#ifdef ENABLE_FULL_MOTION_PLANNER
+
+// Enumeration for G code distance mode
+typedef enum {
+    ABSOLUTE = 90,
+    INCREMENTAL = 91
+} DISTANCE_MODE;
+
+
+// Axis enumeration
+typedef enum {
+    X_AXIS = 'X', // main linear axes
+    Y_AXIS = 'Y',
+    Z_AXIS = 'Z',
+
+    A_AXIS = 'A', // rotary axes
+    B_AXIS = 'B',
+    C_AXIS = 'C',
+
+    U_AXIS = 'U', // additional axes
+    V_AXIS = 'V',
+    W_AXIS = 'W'
+} AXES;
+
+
+// Planner class for planning everything motion
+class Planner {
+
+    public:
+
+        // Initializer
+        Planner();
+
+        // Set distance mode
+        void setDistanceMode(DISTANCE_MODE newDisMode);
+
+        // Get the distance mode
+        DISTANCE_MODE getDistanceMode();
+
+        // Set the default stepping rate for G6
+        void setDefaultSteppingRate(int32_t newRate);
+
+        // Get the default stepping rate for G6
+        int32_t getDefaultSteppingRate();
+
+        // Set the last step rate
+        void setLastStepRate(int32_t rate);
+
+        // Get the last step rate
+        int32_t getLastStepRate();
+
+    private:
+        // Keeps the current G code distance mode
+        DISTANCE_MODE distanceMode = ABSOLUTE;
+
+        #ifdef ENABLE_DIRECT_STEPPING
+        // Keeps the current G code rate for G6 in Hz
+        int32_t defaultRate = DEFAULT_STEPPING_RATE;
+        #endif
+
+        // Keeps the last rate used
+        #ifdef ENABLE_DIRECT_STEPPING
+        int32_t lastRate = DEFAULT_STEPPING_RATE;
+        #else
+        int32_t lastRate = 0;
+        #endif
+};
+
+#endif // ! ENABLE_FULL_MOTION_PLANNER
+#endif // ! __PLANNER_H__
diff --git a/src/software/sanityCheck.h b/src/software/sanityCheck.h
index 9398caaa..14a2a919 100644
--- a/src/software/sanityCheck.h
+++ b/src/software/sanityCheck.h
@@ -152,4 +152,10 @@
 
 #if defined(CHECK_MCO_OUTPUT) && defined(CHECK_GPIO_OUTPUT_SWITCHING)
     #error Only one of the following is allowed at a time: CHECK_MCO_OUTPUT, CHECK_GPIO_OUTPUT_SWITCHING
+#endif
+
+
+// Make sure that ENABLE_FULL_MOTION_PLANNER is defined, otherwise ENABLE_DIRECT_STEPPING can't be used
+#if (defined(ENABLE_DIRECT_STEPPING) && !defined(ENABLE_FULL_MOTION_PLANNER))
+    #error In order to use ENABLE_DIRECT_STEPPING, ENABLE_FULL_MOTION_PLANNER must be uncommented
 #endif
\ No newline at end of file

From f23bc3271d1c9e8845c1f3c7da77480c2de1e651 Mon Sep 17 00:00:00 2001
From: Ihor Nehrutsa <Ihor.Nehrutsa@gmail.com>
Date: Fri, 6 Aug 2021 16:37:03 +0300
Subject: [PATCH 06/11] Negative counts of step is possible (G6 S-1000)

---
 src/software/parser.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index 2cb7e398..3c896c14 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -12,7 +12,7 @@ String parseCommand(String buffer) {
     // Gcode Table
     //  - G90 Absolute positioning // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g90-g91
     //  - G91 incremental positioning // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g90-g91
-    //  - G0 (ex G0 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
+    //  - G0 (ex G0 R1000 A123.45) // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
     //    https://marlinfw.org/docs/gcode/G006.html
     //  - G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move. Requires `ENABLE_DIRECT_STEPPING`
     //  - M17 (ex M17) - Enables the motor (overrides enable pin)
@@ -398,7 +398,7 @@ String parseCommand(String buffer) {
         switch (parseValue(buffer, 'G').toInt()) {
 
             case 0: {
-                // - G0 (ex G0 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units
+                // - G0 (ex G0 R1000 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units
                 // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
                 // Pull the values from the command
                 float value = parseValue(buffer, (char)motor.axis).toFloat();
@@ -467,9 +467,12 @@ String parseCommand(String buffer) {
                 if (rate <= 0) {
                     rate = motor.planner.getDefaultSteppingRate();
                 }
-                if (count <= 0) {
+                if (count == 0) {
                     return FEEDBACK_NO_VALUE;
                 }
+                if (count < 0) {
+                    reverse = not reverse;
+                }
 
                 // Call the steps to be scheduled
                 if (!reverse) {

From f5ba3ea7d6a6267cfe9385ab71878516f83d8c66 Mon Sep 17 00:00:00 2001
From: Christian Piper <42127153+CAP1Sup@users.noreply.github.com>
Date: Fri, 6 Aug 2021 12:14:16 -0400
Subject: [PATCH 07/11] Moved steps/mm to float, changed G0 to conform with
 docs

G0 now uses feedrates in deg/m and mm/m respectively
---
 README.md                |  2 +-
 src/config/config.h      |  2 +-
 src/hardware/motor.cpp   |  6 +++---
 src/hardware/motor.h     | 13 ++++++-------
 src/software/parser.cpp  | 16 ++++++++++++----
 src/software/planner.cpp | 12 ++++++------
 src/software/planner.h   | 14 +++++++-------
 7 files changed, 36 insertions(+), 29 deletions(-)

diff --git a/README.md b/README.md
index 390c743e..3eb7743a 100644
--- a/README.md
+++ b/README.md
@@ -27,7 +27,7 @@ Future Features:
 
 G/M Code Table
 
-- G0 (ex G0 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units. Requires `ENABLE_FULL_MOTION_PLANNER`
+- G0 (ex G0 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units. The feedrate is in units per minute (mm/m for X, Y, and Z, deg/m for A, B, and C). Requires `ENABLE_FULL_MOTION_PLANNER`
 - G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move. Requires `ENABLE_DIRECT_STEPPING`
 - G90 (ex G90) - Sets the movement units in absolute positioning. Requires `ENABLE_FULL_MOTION_PLANNER`
 - G91 (ex G91) - Sets the movement units in relative (incremental) positioning. Requires `ENABLE_FULL_MOTION_PLANNER`
diff --git a/src/config/config.h b/src/config/config.h
index 4c984a68..655a7934 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -115,7 +115,7 @@
 
 // PID settings
 // ! At this time, this feature is still under development
-//#define ENABLE_PID
+#define ENABLE_PID
 #ifdef ENABLE_PID
 
     // Default P, I, and D terms
diff --git a/src/hardware/motor.cpp b/src/hardware/motor.cpp
index 8403e01f..305f6572 100644
--- a/src/hardware/motor.cpp
+++ b/src/hardware/motor.cpp
@@ -355,7 +355,7 @@ void StepperMotor::setMicrostepping(uint16_t setMicrostepping, bool lock) {
 
         // Scale the steps per mm (only used if FULL_MOTION_PLANNER is enabled)
         #ifdef ENABLE_FULL_MOTION_PLANNER
-        this -> stepsPerMM = round(stepsPerMM * stepScalingFactor);
+        this -> stepsPerMM *= stepScalingFactor;
         #endif
 
         // Scale the microstep multiplier so that the full stepping level is maintained
@@ -422,13 +422,13 @@ int32_t StepperMotor::getMicrostepsPerRotation() const {
 // Only needed if FULL_MOTION_PLANNER is enabled
 #ifdef ENABLE_FULL_MOTION_PLANNER
 // Set the steps per mm of the motor
-void StepperMotor::setStepsPerMM(uint16_t newStepsPerMM) {
+void StepperMotor::setStepsPerMM(float newStepsPerMM) {
     this -> stepsPerMM = newStepsPerMM;
 }
 
 
 // Get the steps per mm of the motor
-uint16_t StepperMotor::getStepsPerMM() {
+float StepperMotor::getStepsPerMM() {
     return (this -> stepsPerMM);
 }
 #endif // ! ENABLE_FULL_MOTION_PLANNER
diff --git a/src/hardware/motor.h b/src/hardware/motor.h
index 100bb5d8..eb537f72 100644
--- a/src/hardware/motor.h
+++ b/src/hardware/motor.h
@@ -167,10 +167,10 @@ class StepperMotor {
         // Only needed if FULL_MOTION_PLANNER is enabled
         #ifdef ENABLE_FULL_MOTION_PLANNER
         // Set the steps per mm of the motor
-        void setStepsPerMM(uint16_t newStepsPerMM);
+        void setStepsPerMM(float newStepsPerMM);
 
         // Get the steps per mm of the motor
-        uint16_t getStepsPerMM();
+        float getStepsPerMM();
         #endif // ! ENABLE_FULL_MOTION_PLANNER
 
         // Set if the motor should be reversed
@@ -234,15 +234,14 @@ class StepperMotor {
         // Counter for number of overflows (needs to be public for the interrupt)
         int32_t stepOverflowOffset = 0;
 
-        // Planner (used for motion support)
+        // Motion planner features
         #ifdef ENABLE_FULL_MOTION_PLANNER
+        // Planner (used for motion support)
         Planner planner;
-        #endif
 
         // Motor axis
-        #ifdef ENABLE_FULL_MOTION_PLANNER
         AXES axis = A_AXIS;
-        #endif
+        #endif // ! ENABLE_FULL_MOTION_PLANNER
 
     // Things that shouldn't be accessed by the outside
     private:
@@ -301,7 +300,7 @@ class StepperMotor {
 
         // Variable to save the steps per mm of the motor (only needed if FULL_MOTION_PLANNER is enabled)
         #ifdef ENABLE_FULL_MOTION_PLANNER
-        uint16_t stepsPerMM = 0;
+        float stepsPerMM = 0;
         #endif
 
         // If the motor is enabled or not (saves time so that the enable and disable pins are only set once)
diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index 3c896c14..29504534 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -398,19 +398,19 @@ String parseCommand(String buffer) {
         switch (parseValue(buffer, 'G').toInt()) {
 
             case 0: {
-                // - G0 (ex G0 R1000 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units
+                // - G0 (ex G0 R1000 A123.45) - Rapid movement at a specified distance. Distance can be in degrees (for axes A, B, C) or in mm (for axes X, Y, Z). Steps/mm must be set for movements using mm units. The feedrate is in units per minute (mm/m for X, Y, and Z, deg/m for A, B, and C)
                 // http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g0
                 // Pull the values from the command
                 float value = parseValue(buffer, (char)motor.axis).toFloat();
-                int32_t rate = parseValue(buffer, 'R').toInt();
+                float rate = parseValue(buffer, 'F').toFloat();
 
                 // Sanitize the inputs
                 if (rate <= 0) {
-                    rate = motor.planner.getLastStepRate();
+                    rate = motor.planner.getLastFeedRate();
                 }
 
                 // Save the new rate for next time (needed if the next command doesn't specify rate)
-                motor.planner.setLastStepRate(rate);
+                motor.planner.setLastFeedRate(rate);
 
                 // Determine the number of steps we need to move
                 // Create a count value for number of steps
@@ -421,12 +421,20 @@ String parseCommand(String buffer) {
 
                     // Units are degrees
                     count = round(value / motor.getMicrostepAngle());
+
+                    // Fix the rate so that it is in steps/s instead of deg/m
+                    rate = (rate / (motor.getMicrostepAngle() * 60));
                 }
                 else {
                     // Units must be in mm
                     // Check to make sure that steps per mm has been set
                     if (motor.getStepsPerMM() > 0) {
+
+                        // Set the count value
                         count = round(value * motor.getStepsPerMM());
+
+                        // Fix the rate so that it is in steps/s instead of mm/m
+                        rate = (rate * motor.getStepsPerMM()) / 60;
                     }
                     else {
                         // There is no steps per mm, throw an error
diff --git a/src/software/planner.cpp b/src/software/planner.cpp
index 1948868d..8c37584c 100644
--- a/src/software/planner.cpp
+++ b/src/software/planner.cpp
@@ -23,24 +23,24 @@ DISTANCE_MODE Planner::getDistanceMode() {
 #ifdef ENABLE_DIRECT_STEPPING
 // Set the default stepping rate for G6
 void Planner::setDefaultSteppingRate(int32_t newRate) {
-    this -> defaultRate = newRate;
+    this -> defaultStepRate = newRate;
 }
 
 
 // Get the default stepping rate for G6
 int32_t Planner::getDefaultSteppingRate() {
-    return (this -> defaultRate);
+    return (this -> defaultStepRate);
 }
 #endif // ! ENABLE_DIRECT_STEPPING
 
 // Set the last step rate
-void Planner::setLastStepRate(int32_t rate) {
-    this -> lastRate = rate;
+void Planner::setLastFeedRate(int32_t rate) {
+    this -> lastFeedRate = rate;
 }
 
 // Get the last step rate
-int32_t Planner::getLastStepRate() {
-    return (this -> lastRate);
+int32_t Planner::getLastFeedRate() {
+    return (this -> lastFeedRate);
 }
 
 #endif // ! ENABLE_FULL_MOTION_PLANNER
\ No newline at end of file
diff --git a/src/software/planner.h b/src/software/planner.h
index 5de0def6..93222259 100644
--- a/src/software/planner.h
+++ b/src/software/planner.h
@@ -50,11 +50,11 @@ class Planner {
         // Get the default stepping rate for G6
         int32_t getDefaultSteppingRate();
 
-        // Set the last step rate
-        void setLastStepRate(int32_t rate);
+        // Set the last feed rate
+        void setLastFeedRate(int32_t rate);
 
-        // Get the last step rate
-        int32_t getLastStepRate();
+        // Get the last feed rate
+        int32_t getLastFeedRate();
 
     private:
         // Keeps the current G code distance mode
@@ -62,14 +62,14 @@ class Planner {
 
         #ifdef ENABLE_DIRECT_STEPPING
         // Keeps the current G code rate for G6 in Hz
-        int32_t defaultRate = DEFAULT_STEPPING_RATE;
+        int32_t defaultStepRate = DEFAULT_STEPPING_RATE;
         #endif
 
         // Keeps the last rate used
         #ifdef ENABLE_DIRECT_STEPPING
-        int32_t lastRate = DEFAULT_STEPPING_RATE;
+        int32_t lastFeedRate = DEFAULT_STEPPING_RATE;
         #else
-        int32_t lastRate = 0;
+        int32_t lastFeedRate = 0;
         #endif
 };
 

From 70d8c9374ca5767acaabb10139c7b410fbcf52a4 Mon Sep 17 00:00:00 2001
From: Christian Piper <42127153+CAP1Sup@users.noreply.github.com>
Date: Fri, 6 Aug 2021 12:20:46 -0400
Subject: [PATCH 08/11] Fixed minor issues with inclusion and G6 saved rates

---
 src/software/parser.cpp  | 13 +++++++++----
 src/software/planner.cpp | 14 ++++++++++++--
 src/software/planner.h   | 13 ++++++++++++-
 3 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index 29504534..f29254fa 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -390,7 +390,7 @@ String parseCommand(String buffer) {
     }
 
     // Gcodes support
-    #ifdef ENABLE_DIRECT_STEPPING
+    #ifdef ENABLE_FULL_MOTION_PLANNER
     // Check to see if a gcode exists
     else if (parseValue(buffer, 'G') != "-1") {
 
@@ -464,6 +464,7 @@ String parseCommand(String buffer) {
                 return FEEDBACK_OK;
             }
 
+            #ifdef ENABLE_DIRECT_STEPPING
             case 6: {
                 // G6 (ex G6 D0 R1000 S1000) - Direct stepping, commands the motor to move a specified number of steps in the specified direction. D is direction (0 for CCW, 1 for CW), R is rate (in Hz), and S is the count of steps to move
                 // Pull the values from the command
@@ -473,15 +474,18 @@ String parseCommand(String buffer) {
 
                 // Sanitize the inputs
                 if (rate <= 0) {
-                    rate = motor.planner.getDefaultSteppingRate();
+                    rate = motor.planner.getLastStepRate();
                 }
                 if (count == 0) {
                     return FEEDBACK_NO_VALUE;
                 }
                 if (count < 0) {
-                    reverse = not reverse;
+                    reverse = !reverse;
                 }
 
+                // Save the current rate as the new one
+                motor.planner.setLastStepRate(rate);
+
                 // Call the steps to be scheduled
                 if (!reverse) {
                     scheduleSteps(count, rate, COUNTER_CLOCKWISE);
@@ -493,6 +497,7 @@ String parseCommand(String buffer) {
                 // All good, we can exit
                 return FEEDBACK_OK;
             }
+            #endif
 
             case 90: {
                 motor.planner.setDistanceMode(ABSOLUTE);
@@ -512,7 +517,7 @@ String parseCommand(String buffer) {
 
     }
 
-    #endif
+    #endif // ! ENABLE_FULL_MOTION_PLANNER
 
     // Nothing here, nothing to do
     return FEEDBACK_NO_CMD_SPECIFIED;
diff --git a/src/software/planner.cpp b/src/software/planner.cpp
index 8c37584c..d28c43ac 100644
--- a/src/software/planner.cpp
+++ b/src/software/planner.cpp
@@ -31,14 +31,24 @@ void Planner::setDefaultSteppingRate(int32_t newRate) {
 int32_t Planner::getDefaultSteppingRate() {
     return (this -> defaultStepRate);
 }
-#endif // ! ENABLE_DIRECT_STEPPING
 
 // Set the last step rate
+void Planner::setLastStepRate(int32_t rate) {
+    this -> lastStepRate = rate;
+}
+
+// Get the last step rate
+int32_t Planner::getLastStepRate() {
+    return (this -> lastStepRate);
+}
+#endif // ! ENABLE_DIRECT_STEPPING
+
+// Set the last feed rate
 void Planner::setLastFeedRate(int32_t rate) {
     this -> lastFeedRate = rate;
 }
 
-// Get the last step rate
+// Get the last feed rate
 int32_t Planner::getLastFeedRate() {
     return (this -> lastFeedRate);
 }
diff --git a/src/software/planner.h b/src/software/planner.h
index 93222259..40ede16c 100644
--- a/src/software/planner.h
+++ b/src/software/planner.h
@@ -44,12 +44,20 @@ class Planner {
         // Get the distance mode
         DISTANCE_MODE getDistanceMode();
 
+        #ifdef ENABLE_DIRECT_STEPPING
         // Set the default stepping rate for G6
         void setDefaultSteppingRate(int32_t newRate);
 
         // Get the default stepping rate for G6
         int32_t getDefaultSteppingRate();
 
+        // Set the last step rate
+        void setLastStepRate(int32_t rate);
+
+        // Get the last step rate
+        int32_t getLastStepRate();
+        #endif // ! ENABLE_DIRECT_STEPPING
+
         // Set the last feed rate
         void setLastFeedRate(int32_t rate);
 
@@ -61,8 +69,11 @@ class Planner {
         DISTANCE_MODE distanceMode = ABSOLUTE;
 
         #ifdef ENABLE_DIRECT_STEPPING
-        // Keeps the current G code rate for G6 in Hz
+        // Keeps the default G code rate for G6 in Hz
         int32_t defaultStepRate = DEFAULT_STEPPING_RATE;
+
+        // Keeps the last step rate for G6 in Hz
+        int32_t lastStepRate = DEFAULT_STEPPING_RATE;
         #endif
 
         // Keeps the last rate used

From 444eee224e3b3f065a2d70a65b0999f1d361ff18 Mon Sep 17 00:00:00 2001
From: Christian Piper <42127153+CAP1Sup@users.noreply.github.com>
Date: Fri, 6 Aug 2021 12:28:14 -0400
Subject: [PATCH 09/11] Fixed missing references causing build to fail

---
 src/hardware/motor.cpp  | 17 -----------------
 src/software/parser.cpp |  4 ++--
 2 files changed, 2 insertions(+), 19 deletions(-)

diff --git a/src/hardware/motor.cpp b/src/hardware/motor.cpp
index 21692d99..df4a60c5 100644
--- a/src/hardware/motor.cpp
+++ b/src/hardware/motor.cpp
@@ -535,23 +535,6 @@ void StepperMotor::step(STEP_DIR dir, bool useMultiplier, bool updateDesiredPos)
         stepChange = (this -> microstepMultiplier);
     }
 
-    /*
-    // Invert the change based on the direction
-    if (dir == PIN) {
-
-        // Use the DIR_PIN state to decide direction
-        stepChange *= (DIRECTION(GPIO_READ(DIRECTION_PIN)) * (this -> reversed));
-    }
-    //else if (dir == COUNTER_CLOCKWISE) {
-        // Nothing to do here, the value is already positive
-    //}
-    else if (dir == CLOCKWISE) {
-
-        // Make the step change in the negative direction
-        stepChange = -stepChange;
-    }
-    */
-
     #ifdef ENABLE_STEPPING_VELOCITY
         isStepping = false;
     #endif
diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index 262c9da5..0f8ec1f5 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -448,13 +448,13 @@ String parseCommand(String buffer) {
                 }
 
                 // Default to CCW rotation
-                STEP_DIR dir = COUNTER_CLOCKWISE;
+                STEP_DIR dir = POSITIVE;
 
                 // If the count is negative (motor needs to move in opposite direction)
                 // then we can make the count positive and fix the direction
                 if (count < 0) {
                     count = -count;
-                    dir = CLOCKWISE;
+                    dir = NEGATIVE;
                 }
 
                 // Schedule the steps to be moved

From c3156afe32a85e497dec5af1e0118f9d390e9b8d Mon Sep 17 00:00:00 2001
From: Christian Piper <42127153+CAP1Sup@users.noreply.github.com>
Date: Fri, 6 Aug 2021 12:33:08 -0400
Subject: [PATCH 10/11] Fixed inclusion issues due to legacy options

---
 src/hardware/timers.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/hardware/timers.cpp b/src/hardware/timers.cpp
index 39128035..9231ad8d 100644
--- a/src/hardware/timers.cpp
+++ b/src/hardware/timers.cpp
@@ -44,7 +44,7 @@ static uint8_t interruptBlockCount = 0;
 
 
 // Setup everything related to step scheduling
-#if (defined(ENABLE_DIRECT_STEPPING) || defined(ENABLE_PID))
+#if (defined(ENABLE_FULL_MOTION_PLANNER) || defined(ENABLE_PID))
 
     // Main timer for scheduling steps
     HardwareTimer *stepScheduleTimer = new HardwareTimer(TIM4);
@@ -75,7 +75,7 @@ void setupMotorTimers() {
     // - 5 - hardware step counter overflow handling
     // - 6 - step pin change
     // - 7.0 - position correction (or PID interval update)
-    // - 7.1 - scheduled steps (if ENABLE_DIRECT_STEPPING or ENABLE_PID)
+    // - 7.1 - scheduled steps (if ENABLE_FULL_MOTION_PLANNER or ENABLE_PID)
 
     // Check if StallFault is enabled
     #ifdef ENABLE_STALLFAULT
@@ -108,7 +108,7 @@ void setupMotorTimers() {
     #endif // ! DISABLE_CORRECTION_TIMER
 
     // Setup step schedule timer if it is enabled
-    #if (defined(ENABLE_DIRECT_STEPPING) || defined(ENABLE_PID))
+    #if (defined(ENABLE_FULL_MOTION_PLANNER) || defined(ENABLE_PID))
         stepScheduleTimer -> pause();
         stepScheduleTimer -> setInterruptPriority(7, 1);
         stepScheduleTimer -> setMode(1, TIMER_OUTPUT_COMPARE); // Disables the output, since we only need the timed interrupt
@@ -133,7 +133,7 @@ void disableMotorTimers() {
     #endif
 
     // Disable the stepping timer if it is enabled
-    #if (defined(ENABLE_DIRECT_STEPPING) || defined(ENABLE_PID))
+    #if (defined(ENABLE_FULL_MOTION_PLANNER) || defined(ENABLE_PID))
     disableStepScheduleTimer();
     #endif
 }
@@ -214,7 +214,7 @@ void disableStepCorrection() {
     }
 
     // Disable the stepping timer if needed
-    #if (defined(ENABLE_DIRECT_STEPPING) || defined(ENABLE_PID))
+    #if (defined(ENABLE_FULL_MOTION_PLANNER) || defined(ENABLE_PID))
     disableStepScheduleTimer();
     #endif
 }
@@ -455,7 +455,7 @@ void correctMotor() {
 
 
 // Direct stepping
-#ifdef ENABLE_DIRECT_STEPPING
+#ifdef ENABLE_FULL_MOTION_PLANNER
 // Configure a specific number of steps to execute at a set rate (rate is in Hz)
 void scheduleSteps(int64_t count, int32_t rate, STEP_DIR stepDir) {
 
@@ -476,7 +476,7 @@ void scheduleSteps(int64_t count, int32_t rate, STEP_DIR stepDir) {
 }
 #endif
 
-#if (defined(ENABLE_DIRECT_STEPPING) || defined(ENABLE_PID))
+#if (defined(ENABLE_FULL_MOTION_PLANNER) || defined(ENABLE_PID))
 // Handles a step schedule event
 void stepScheduleHandler() {
 
@@ -533,7 +533,7 @@ void disableStepScheduleTimer() {
         syncInstructions();
     }
 }
-#endif // ! ENABLE_DIRECT_STEPPING
+#endif // ! ENABLE_FULL_MOTION_PLANNER || ENABLE_PID
 
 
 // Makes sure that all cached calls respect the current config

From 6c9e983a80044459b49c49f9256c731acdcbad7f Mon Sep 17 00:00:00 2001
From: Christian Piper <42127153+CAP1Sup@users.noreply.github.com>
Date: Fri, 6 Aug 2021 12:39:30 -0400
Subject: [PATCH 11/11] Finally fixed legacy options issue

---
 src/hardware/timers.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/hardware/timers.h b/src/hardware/timers.h
index f9393c7f..04322e60 100644
--- a/src/hardware/timers.h
+++ b/src/hardware/timers.h
@@ -54,12 +54,12 @@ void stepMotorNoDesiredAngle();
 void correctMotor();
 
 // Direct stepping
-#ifdef ENABLE_DIRECT_STEPPING
+#ifdef ENABLE_FULL_MOTION_PLANNER
 // Schedule steps for the motor to execute (rate is in Hz)
 void scheduleSteps(int64_t count, int32_t rate, STEP_DIR stepDir);
-#endif // ! ENABLE_DIRECT_STEPPING
+#endif // ! ENABLE_FULL_MOTION_PLANNER
 
-#if (defined(ENABLE_DIRECT_STEPPING) || defined(ENABLE_PID))
+#if (defined(ENABLE_FULL_MOTION_PLANNER) || defined(ENABLE_PID))
 // Step schedule handler (runs when the interrupt is triggered)
 void stepScheduleHandler();
 
@@ -68,7 +68,7 @@ void enableStepScheduleTimer();
 
 // Convenience function to handle disabling the step schedule timer
 void disableStepScheduleTimer();
-#endif // ! ENABLE_DIRECT_STEPPING || ENABLE_PID
+#endif // ! ENABLE_FULL_MOTION_PLANNER || ENABLE_PID
 
 // Makes sure that all cached calls respect the current config
 void syncInstructions();