diff --git a/README.md b/README.md
index e7d5f6ea..3eb7743a 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. 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`
 - 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 82f557be..edf76a20 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 017fa09c..98a4b16c 100644
--- a/src/hardware/motor.cpp
+++ b/src/hardware/motor.cpp
@@ -353,6 +353,11 @@ void StepperMotor::setMicrostepping(uint8_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 *= 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
@@ -417,6 +422,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(float newStepsPerMM) {
+    this -> stepsPerMM = newStepsPerMM;
+}
+
+
+// Get the steps per mm of the motor
+float 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) {
@@ -517,22 +535,6 @@ void StepperMotor::step(STEP_DIR dir, int32_t stepChange, 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;
diff --git a/src/hardware/motor.h b/src/hardware/motor.h
index 1e64c9e9..95041c2e 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 "planner.h"
 #include "stm32f1xx_hal_tim.h"
 
 // For sin() and fmod() function
@@ -172,6 +173,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(float newStepsPerMM);
+
+        // Get the steps per mm of the motor
+        float getStepsPerMM();
+        #endif // ! ENABLE_FULL_MOTION_PLANNER
+
         // Set if the motor should be reversed
         void setReversed(bool reversed);
 
@@ -234,6 +244,15 @@ class StepperMotor {
         // TIM2 -> CNT is unsigned, stepOverflowOffset is unsigned, but ((TIM2 -> CNT) + stepOverflowOffset) is treated as signed value
         uint32_t stepOverflowOffset = 0;
 
+        // Motion planner features
+        #ifdef ENABLE_FULL_MOTION_PLANNER
+        // Planner (used for motion support)
+        Planner planner;
+
+        // Motor axis
+        AXES axis = A_AXIS;
+        #endif // ! ENABLE_FULL_MOTION_PLANNER
+
         // Microstep multiplier (used to move a custom number of microsteps per step pulse)
         uint32_t microstepMultiplier = DEFAULT_MICROSTEP_MULTIPLIER;
 
@@ -292,6 +311,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
+        float stepsPerMM = 0;
+        #endif
+  
         // Step to sine array conversion
         int32_t stepToSineArrayFactor = MAX_MICROSTEP_DIVISOR / getMicrostepping();
 
diff --git a/src/hardware/timers.cpp b/src/hardware/timers.cpp
index aa64e694..54b6a9db 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
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();
diff --git a/src/software/parser.cpp b/src/software/parser.cpp
index baa49579..0f8ec1f5 100644
--- a/src/software/parser.cpp
+++ b/src/software/parser.cpp
@@ -10,6 +10,11 @@
 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 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)
     //  - 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.
@@ -385,13 +390,81 @@ 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") {
 
         // Switch statement the command number
         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. 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();
+                float rate = parseValue(buffer, 'F').toFloat();
+
+                // Sanitize the inputs
+                if (rate <= 0) {
+                    rate = motor.planner.getLastFeedRate();
+                }
+
+                // Save the new rate for next time (needed if the next command doesn't specify rate)
+                motor.planner.setLastFeedRate(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());
+
+                    // 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
+                        return FEEDBACK_STEPS_PER_MM_NOT_SET;
+                    }
+                }
+
+                // Adjust the count by the current position if the mode is absolute
+                if (motor.planner.getDistanceMode() == ABSOLUTE) {
+                    count -= motor.getDesiredStep();
+                }
+
+                // Default to CCW rotation
+                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 = NEGATIVE;
+                }
+
+                // Schedule the steps to be moved
+                scheduleSteps(count, rate, dir);
+
+                // Return that everything went well
+                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
@@ -401,11 +474,17 @@ String parseCommand(String buffer) {
 
                 // Sanitize the inputs
                 if (rate <= 0) {
-                    rate = DEFAULT_STEPPING_RATE;
+                    rate = motor.planner.getLastStepRate();
                 }
-                if (count <= 0) {
+                if (count == 0) {
                     return FEEDBACK_NO_VALUE;
                 }
+                if (count < 0) {
+                    reverse = !reverse;
+                }
+
+                // Save the current rate as the new one
+                motor.planner.setLastStepRate(rate);
 
                 // Call the steps to be scheduled
                 if (!reverse) {
@@ -418,6 +497,17 @@ String parseCommand(String buffer) {
                 // All good, we can exit
                 return FEEDBACK_OK;
             }
+            #endif
+
+            case 90: {
+                motor.planner.setDistanceMode(ABSOLUTE);
+                return FEEDBACK_OK;
+            }
+
+            case 91: {
+                motor.planner.setDistanceMode(INCREMENTAL);
+                return FEEDBACK_OK;
+            }
 
             default: {
                 // Command isn't recognized, therefore throw an error
@@ -427,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/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..d28c43ac
--- /dev/null
+++ b/src/software/planner.cpp
@@ -0,0 +1,56 @@
+// 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 -> defaultStepRate = newRate;
+}
+
+
+// Get the default stepping rate for G6
+int32_t Planner::getDefaultSteppingRate() {
+    return (this -> defaultStepRate);
+}
+
+// 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 feed rate
+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
new file mode 100644
index 00000000..40ede16c
--- /dev/null
+++ b/src/software/planner.h
@@ -0,0 +1,88 @@
+#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();
+
+        #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);
+
+        // Get the last feed rate
+        int32_t getLastFeedRate();
+
+    private:
+        // Keeps the current G code distance mode
+        DISTANCE_MODE distanceMode = ABSOLUTE;
+
+        #ifdef ENABLE_DIRECT_STEPPING
+        // 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
+        #ifdef ENABLE_DIRECT_STEPPING
+        int32_t lastFeedRate = DEFAULT_STEPPING_RATE;
+        #else
+        int32_t lastFeedRate = 0;
+        #endif
+};
+
+#endif // ! ENABLE_FULL_MOTION_PLANNER
+#endif // ! __PLANNER_H__
diff --git a/src/software/sanityCheck.h b/src/software/sanityCheck.h
index aa27aa7e..565b070c 100644
--- a/src/software/sanityCheck.h
+++ b/src/software/sanityCheck.h
@@ -151,6 +151,12 @@
 #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
+
+
 // Microstep checks (makes sure that the min and max values are within viable ranges)
 #if (IS_POWER_2(MIN_MICROSTEP_DIVISOR) != 0)
 
@@ -177,4 +183,4 @@
 
 
 // Calculate the microstep interval count
-#define MICROSTEP_INTERVAL_CNT (uint16_t)(log2(MAX_MICROSTEP_DIVISOR) - log2(MIN_MICROSTEP_DIVISOR) + 1)
\ No newline at end of file
+#define MICROSTEP_INTERVAL_CNT (uint16_t)(log2(MAX_MICROSTEP_DIVISOR) - log2(MIN_MICROSTEP_DIVISOR) + 1)