Add G0, G90, G91

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.

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"

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

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;

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();
 

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

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();