From 21b45327887d228d65d967857ac77b6b883b34fc Mon Sep 17 00:00:00 2001
From: lavolpecheprogramma <parziale.alberto@gmail.com>
Date: Mon, 10 Feb 2020 22:45:28 +0100
Subject: [PATCH] add servo support by DWiskow

---
 grbl/config.h          |  6 +++---
 grbl/cpu_map.h         |  3 ++-
 grbl/spindle_control.c | 35 ++++++++++++++++++++++++++++++++++-
 3 files changed, 39 insertions(+), 5 deletions(-)

diff --git a/grbl/config.h b/grbl/config.h
index f48d958..dfa9355 100644
--- a/grbl/config.h
+++ b/grbl/config.h
@@ -102,12 +102,12 @@
 // on separate pin, but homed in one cycle. Also, it should be noted that the function of hard limits
 // will not be affected by pin sharing.
 // NOTE: Defaults are set for a traditional 3-axis CNC machine. Z-axis first to clear, followed by X & Y.
-#define HOMING_CYCLE_0 (1<<Z_AXIS)                // REQUIRED: First move Z to clear workspace.
-#define HOMING_CYCLE_1 ((1<<X_AXIS)|(1<<Y_AXIS))  // OPTIONAL: Then move X,Y at the same time.
+// #define HOMING_CYCLE_0 (1<<Z_AXIS)                // REQUIRED: First move Z to clear workspace.
+// #define HOMING_CYCLE_1 ((1<<X_AXIS)|(1<<Y_AXIS))  // OPTIONAL: Then move X,Y at the same time.
 // #define HOMING_CYCLE_2                         // OPTIONAL: Uncomment and add axes mask to enable
 
 // NOTE: The following are two examples to setup homing for 2-axis machines.
-// #define HOMING_CYCLE_0 ((1<<X_AXIS)|(1<<Y_AXIS))  // NOT COMPATIBLE WITH COREXY: Homes both X-Y in one cycle. 
+#define HOMING_CYCLE_0 ((1<<X_AXIS)|(1<<Y_AXIS))  // NOT COMPATIBLE WITH COREXY: Homes both X-Y in one cycle. 
 
 // #define HOMING_CYCLE_0 (1<<X_AXIS)  // COREXY COMPATIBLE: First home X
 // #define HOMING_CYCLE_1 (1<<Y_AXIS)  // COREXY COMPATIBLE: Then home Y
diff --git a/grbl/cpu_map.h b/grbl/cpu_map.h
index 9f43a3f..3001d82 100644
--- a/grbl/cpu_map.h
+++ b/grbl/cpu_map.h
@@ -144,7 +144,8 @@
     // #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)
+    // #define SPINDLE_TCCRB_INIT_MASK      (1<<CS22)               // 1/64 prescaler -> 0.98kHz (J-tech laser)
+    #define SPINDLE_TCCRB_INIT_MASK      ((1<<CS22) | (1<<CS21) | (1<<CS20)) // 1/1024 prescaler -> 61Hz (for Servo)
 
     // NOTE: On the 328p, these must be the same as the SPINDLE_ENABLE settings.
     #define SPINDLE_PWM_DDR   DDRB
diff --git a/grbl/spindle_control.c b/grbl/spindle_control.c
index 550b752..4671ff2 100644
--- a/grbl/spindle_control.c
+++ b/grbl/spindle_control.c
@@ -21,6 +21,10 @@
 
 #include "grbl.h"
 
+#define RC_SERVO_SHORT      15      // set min pulse duration to (7 = 0.5ms, 15 = 1.03ms, 20=1.40ms)    // RC Servo
+#define RC_SERVO_LONG       31      // set max pulse duration (38 = 2.49ms, 31 = 2.05ms)                // RC Servo
+#define RC_SERVO_RANGE      (RC_SERVO_LONG-RC_SERVO_SHORT)                                              // RC Servo
+// #define RC_SERVO_INVERT  1       // Uncomment to invert servo direction                              // RC Servo
 
 #ifdef VARIABLE_SPINDLE
   static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
@@ -113,6 +117,16 @@ void spindle_stop()
       SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
     #endif
   #endif
+  if (!(settings.flags & BITFLAG_LASER_MODE)) {                                                         // RC Servo
+  #ifdef RC_SERVO_SHORT                                                                                 // RC Servo
+    SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.                   // RC Servo
+    #ifdef RC_SERVO_INVERT                                                                              // RC Servo
+      SPINDLE_OCR_REGISTER = RC_SERVO_LONG;                                                             // RC Servo
+    #else                                                                                               // RC Servo
+      SPINDLE_OCR_REGISTER = RC_SERVO_SHORT;                                                            // RC Servo
+    #endif                                                                                              // RC Servo
+  #endif                                                                                                // RC Servo
+  }                                                                                                     // RC Servo
 }
 
 
@@ -135,7 +149,15 @@ 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)) {                                                   // RC Servo
+          #ifndef RC_SERVO_SHORT                                                                        // RC Servo
+            SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.   // RC Servo
+          #else                                                                                         // RC Servo
+            spindle_stop();                                                                             // RC Servo
+          #endif                                                                                        // RC Servo
+        } else {                                                                                        // RC Servo
+          SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.     // RC Servo
+        }                                                                                               // RC Servo
       } else {
         SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
       }
@@ -212,6 +234,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)) {                                                         // RC Servo
+        #ifdef RC_SERVO_SHORT                                                                               // RC Servo
+          #ifdef RC_SERVO_INVERT                                                                            // RC Servo
+            pwm_value = floor(RC_SERVO_LONG - rpm*(RC_SERVO_RANGE/(settings.rpm_max-settings.rpm_min)));    // RC Servo
+          #else                                                                                             // RC Servo
+            pwm_value = floor(rpm*(RC_SERVO_RANGE/(settings.rpm_max-settings.rpm_min))+RC_SERVO_SHORT);     // RC Servo
+          #endif                                                                                            // RC Servo
+        #endif                                                                                              // RC Servo
+      }                                                                                                     // RC Servo
+
       return(pwm_value);
     }