From 2165218f1349fd9b637ae7b78c286e815fdfcef5 Mon Sep 17 00:00:00 2001
From: lavolpecheprogramma <parziale.alberto@gmail.com>
Date: Tue, 11 Feb 2020 22:17:16 +0100
Subject: [PATCH] add servo motor support using cprezzi and DWiskow approach

---
 grbl/config.h          | 10 ++++++++++
 grbl/cpu_map.h         | 15 ++++++++++-----
 grbl/spindle_control.c | 34 +++++++++++++++++++++++++++++++---
 3 files changed, 51 insertions(+), 8 deletions(-)

diff --git a/grbl/config.h b/grbl/config.h
index f48d9586..f66a96d1 100644
--- a/grbl/config.h
+++ b/grbl/config.h
@@ -367,6 +367,16 @@
 // NOTE: Requires USE_SPINDLE_DIR_AS_ENABLE_PIN to be enabled.
 // #define SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED // Default disabled. Uncomment to enable.
 
+// If you use a servo instead of a spindle (like on EggBot or pen plotter), you need to uncomment this option. 
+// This will set the PWM frequency to 61Hz and limit the PWM range to 0.5 - 2.5ms, as used by most servos.
+// #define SPINDLE_IS_SERVO // Default disabled. Uncomment to enable.
+
+#define SERVO_SHORT      7      // set min pulse duration to (7 = 0.5ms, 15 = 1.03ms, 20=1.40ms)
+#define SERVO_LONG       38      // set max pulse duration (38 = 2.49ms, 31 = 2.05ms)
+#define SERVO_RANGE      (SERVO_LONG-SERVO_SHORT)
+// #define SERVO_INVERT  1       // Uncomment to invert servo direction
+
+
 // With this enabled, Grbl sends back an echo of the line it has received, which has been pre-parsed (spaces
 // removed, capitalized letters, no comments) and is to be immediately executed by Grbl. Echoes will not be
 // sent upon a line buffer overflow, but should for all normal lines sent to Grbl. For example, if a user
diff --git a/grbl/cpu_map.h b/grbl/cpu_map.h
index 9f43a3fc..529d2ded 100644
--- a/grbl/cpu_map.h
+++ b/grbl/cpu_map.h
@@ -141,11 +141,16 @@
 
     // Prescaled, 8-bit Fast PWM mode.
     #define SPINDLE_TCCRA_INIT_MASK   ((1<<WGM20) | (1<<WGM21))  // Configures fast PWM mode.
-    // #define SPINDLE_TCCRB_INIT_MASK   (1<<CS20)               // Disable prescaler -> 62.5kHz
-    // #define SPINDLE_TCCRB_INIT_MASK   (1<<CS21)               // 1/8 prescaler -> 7.8kHz (Used in v0.9)
-    // #define SPINDLE_TCCRB_INIT_MASK   ((1<<CS21) | (1<<CS20)) // 1/32 prescaler -> 1.96kHz
-    #define SPINDLE_TCCRB_INIT_MASK      (1<<CS22)               // 1/64 prescaler -> 0.98kHz (J-tech laser)
-
+    
+    #ifdef SPINDLE_IS_SERVO
+      #define SPINDLE_TCCRB_INIT_MASK      ((1<<CS22) | (1<<CS21) | (1<<CS20)) // 1/1024 prescaler -> 61Hz (for Servo)
+    #else
+      // #define SPINDLE_TCCRB_INIT_MASK   (1<<CS20)               // Disable prescaler -> 62.5kHz
+      // #define SPINDLE_TCCRB_INIT_MASK   (1<<CS21)               // 1/8 prescaler -> 7.8kHz (Used in v0.9)
+      // #define SPINDLE_TCCRB_INIT_MASK   ((1<<CS21) | (1<<CS20)) // 1/32 prescaler -> 1.96kHz
+      #define SPINDLE_TCCRB_INIT_MASK      (1<<CS22)               // 1/64 prescaler -> 0.98kHz (J-tech laser)
+    #endif
+    
     // NOTE: On the 328p, these must be the same as the SPINDLE_ENABLE settings.
     #define SPINDLE_PWM_DDR   DDRB
     #define SPINDLE_PWM_PORT  PORTB
diff --git a/grbl/spindle_control.c b/grbl/spindle_control.c
index 550b7520..594153f9 100644
--- a/grbl/spindle_control.c
+++ b/grbl/spindle_control.c
@@ -21,7 +21,6 @@
 
 #include "grbl.h"
 
-
 #ifdef VARIABLE_SPINDLE
   static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
 #endif
@@ -113,6 +112,16 @@ void spindle_stop()
       SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
     #endif
   #endif
+  if (!(settings.flags & BITFLAG_LASER_MODE)) {                                                         
+    #ifdef SPINDLE_IS_SERVO
+    SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
+      #ifdef SERVO_INVERT                                                                              
+        SPINDLE_OCR_REGISTER = SERVO_LONG;
+      #else                                                                                               
+        SPINDLE_OCR_REGISTER = SERVO_SHORT;
+      #endif
+    #endif
+  }   
 }
 
 
@@ -135,11 +144,19 @@ void spindle_stop()
       }
     #else
       if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
-        SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
+        if (!(settings.flags & BITFLAG_LASER_MODE)) {                                                
+          #ifndef SPINDLE_IS_SERVO                                                                     
+            SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
+          #else                                                                                      
+            spindle_stop();                                                                          
+          #endif                                                                                     
+        } else {                                                                                     
+          SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.  
+        }
       } else {
         SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
       }
-    #endif
+    #endif                                                                                                  
   }
 
 
@@ -212,6 +229,17 @@ void spindle_stop()
         sys.spindle_speed = rpm;
         pwm_value = floor((rpm-settings.rpm_min)*pwm_gradient) + SPINDLE_PWM_MIN_VALUE;
       }
+      
+      if (!(settings.flags & BITFLAG_LASER_MODE)) {
+        #ifdef SPINDLE_IS_SERVO
+          #ifdef SERVO_INVERT
+            pwm_value = floor(SERVO_LONG - rpm*(SERVO_RANGE/(settings.rpm_max-settings.rpm_min)));
+          #else
+            pwm_value = floor(rpm*(SERVO_RANGE/(settings.rpm_max-settings.rpm_min))+SERVO_SHORT);
+          #endif
+        #endif
+      }
+
       return(pwm_value);
     }