Invert servo PWM mapping and fix M5 behavior

- M5 now disables PWM completely in both standard and servo modes
- Inverted servo mapping: S0=pen up (16 ticks), S1000=pen down (31 ticks)
- Updated SERVO_MODE.md documentation to reflect new behavior
- Changed config defaults: MIN_PULSE=16, MAX_PULSE=31

SERVO_MODE.md

@@ -0,0 +1,203 @@
+# GRBL Servo Mode for Spindle PWM
+
+This modification adds servo control capability to GRBL's spindle PWM output, allowing you to control hobby servos instead of traditional spindles. This is useful for applications like:
+
+- Pen plotters (pen up/down)
+- Drag knives
+- Laser focusing mechanisms
+- Any application requiring servo positioning
+
+## How It Works
+
+### Standard Spindle Mode (Default)
+- **Frequency**: ~7.8kHz (1/8 prescaler)
+- **Duty Cycle**: 0-255 (full range)
+- **Use Case**: Variable speed spindle control
+
+### Servo Mode (When Enabled)
+- **Frequency**: ~61Hz (1/1024 prescaler) 
+  - Target: 50Hz (20ms period) for servos
+  - Actual: 61Hz works well with most hobby servos
+- **Duty Cycle**: 16-31 ticks (~1-2ms pulses)
+  - 16 ticks ≈ 1.024ms (0° position)
+  - 31 ticks ≈ 1.984ms (180° position)
+- **Use Case**: Servo position control
+
+## Configuration
+
+### Enable Servo Mode
+
+In [`config.h`](grbl/config.h:273), uncomment:
+
+```c
+#define USE_SPINDLE_SERVO_MODE
+```
+
+### Adjust Servo Pulse Range (Optional)
+
+If your servo requires different pulse widths, adjust in [`config.h`](grbl/config.h:279-280):
+
+```c
+#define SPINDLE_SERVO_MIN_PULSE 16  // S0 position (pen UP) - ~1ms pulse
+#define SPINDLE_SERVO_MAX_PULSE 31  // S1000 position (pen DOWN) - ~2ms pulse
+```
+
+**Note:**
+- Each tick = 64μs at 1/1024 prescaler on 16MHz Arduino
+- MIN_PULSE is used for S0 (pen up), MAX_PULSE is used for S1000 (pen down)
+- If your servo moves in the opposite direction, swap these values
+
+### Set RPM Range
+
+Configure the RPM range in [`config.h`](grbl/config.h:256-257) to map to servo positions:
+
+```c
+#define SPINDLE_MAX_RPM 1000.0  // Maps to max servo position (180°)
+#define SPINDLE_MIN_RPM 0.0     // Maps to min servo position (0°)
+```
+
+## Usage
+
+Send G-code commands to control servo position:
+
+```gcode
+M3 S0      ; Pen UP (uses SPINDLE_SERVO_MIN_PULSE = 16 ticks / ~1ms)
+M3 S500    ; Mid position
+M3 S1000   ; Pen DOWN (uses SPINDLE_SERVO_MAX_PULSE = 31 ticks / ~2ms)
+M5         ; Disable PWM (servo unpowered)
+```
+
+**Note:** In servo mode, M5 disables the PWM output completely, which unpowers the servo. Use M3 S0 to keep the servo powered at the minimum position (pen up).
+
+The S value is linearly mapped (inverted):
+- **M3 S0** → Uses SPINDLE_SERVO_MIN_PULSE (pen up / 16 ticks / ~1ms)
+- **M3 S1000** → Uses SPINDLE_SERVO_MAX_PULSE (pen down / 31 ticks / ~2ms)
+- **M5** → Disables PWM (servo unpowered)
+- Values in between are linearly interpolated
+
+## Technical Details
+
+### Timer Configuration
+
+**Standard Mode:**
+```c
+TCCRB_REGISTER = 0x02  // CS21=1: 1/8 prescaler
+Frequency = 16MHz / (8 * 256) = 7,812.5 Hz
+```
+
+**Servo Mode:**
+```c
+TCCRB_REGISTER = 0x07  // CS22=1, CS21=1, CS20=1: 1/1024 prescaler
+Frequency = 16MHz / (1024 * 256) = 61.04 Hz (period ≈ 16.38ms)
+```
+
+### PWM Calculation
+
+**Standard Mode:**
+```
+current_pwm = floor(rpm * (255 / RPM_RANGE) + 0.5)
+Range: 0-255
+```
+
+**Servo Mode (Inverted):**
+```
+inverted_rpm = MAX_RPM - rpm
+current_pwm = floor(inverted_rpm * (15 / RPM_RANGE) + 16 + 0.5)
+Range: 16-31 (15 discrete positions)
+S0 → 16 (pen up), S1000 → 31 (pen down)
+```
+
+### Resolution
+
+With the default configuration:
+- **Servo pulse range**: 1.024ms - 1.984ms (960μs range)
+- **Step size**: 64μs per tick
+- **Positions**: 16 discrete positions (15 steps)
+
+This provides sufficient resolution for typical pen plotter and similar applications where precise servo positioning is not critical.
+
+## Hardware Connection
+
+1. Connect servo signal wire to Arduino Pin D11 (PWM output)
+2. Connect servo power (5V) to appropriate power source
+3. Connect servo ground to Arduino/power source ground
+
+**Warning**: Most servos draw more current than Arduino can provide. Use external 5V power supply with common ground.
+
+## Compatibility
+
+- ✅ **Arduino Uno** (ATmega328p) - Fully supported
+- ❌ **Arduino Mega** (ATmega2560) - Not supported (uses Timer4)
+- ❌ Other processors - Not tested
+
+## References
+
+Based on the excellent work by Bart Dring:
+- Blog post: [Using Grbl's Spindle PWM to Control a Servo](https://www.buildlog.net/blog/2017/08/using-grbls-spindle-pwm-to-control-a-servo/)
+- GitHub repo: [Grbl_Pen_Servo](https://github.com/bdring/Grbl_Pen_Servo)
+
+## Implementation Files
+
+- [`config.h`](grbl/config.h:273-281) - Configuration options
+- [`spindle_control.c`](grbl/spindle_control.c:85-125) - Servo mode logic
+- [`cpu_map/cpu_map_atmega328p.h`](grbl/cpu_map/cpu_map_atmega328p.h:129-147) - Timer register definitions
+
+## Troubleshooting
+
+### Servo jitters or doesn't move smoothly
+- Check power supply - servos need stable 5V
+- Verify pulse width range matches your servo specs
+- Some servos may require different MIN/MAX pulse values
+
+### Servo moves to wrong positions
+- Adjust `SPINDLE_SERVO_MIN_PULSE` and `SPINDLE_SERVO_MAX_PULSE`
+- Check that `SPINDLE_MIN_RPM` and `SPINDLE_MAX_RPM` are set correctly
+- Verify S values in G-code are within MIN/MAX RPM range
+
+### Servo doesn't respond at all
+- Verify `VARIABLE_SPINDLE` is enabled
+- Check `USE_SPINDLE_SERVO_MODE` is uncommented
+- Confirm connection to Pin D11
+- Test PWM output with logic analyzer or oscilloscope
+
+## Example: Pen Plotter
+
+```c
+// config.h settings for pen plotter
+#define VARIABLE_SPINDLE
+#define USE_SPINDLE_SERVO_MODE
+#define SPINDLE_MAX_RPM 1000.0
+#define SPINDLE_MIN_RPM 0.0
+#define SPINDLE_SERVO_MIN_PULSE 16  // S0 = Pen UP (~1ms pulse)
+#define SPINDLE_SERVO_MAX_PULSE 31  // S1000 = Pen DOWN (~2ms pulse)
+```
+
+```gcode
+; Pen up (keeping servo powered)
+M3 S0
+
+; Move to start position
+G0 X10 Y10
+
+; Pen down
+M3 S1000
+
+; Draw square
+G1 X20 Y10 F1000
+G1 X20 Y20
+G1 X10 Y20
+G1 X10 Y10
+
+; Pen up (keeping servo powered)
+M3 S0
+```
+
+**Pen Plotter Commands:**
+- **M3 S0** = Pen UP (16 ticks / ~1ms / MIN_PULSE - servo powered)
+- **M3 S1000** = Pen DOWN (31 ticks / ~2ms / MAX_PULSE - servo powered)
+- **M5** = Disable PWM (servo unpowered)
+
+**Servo Direction:**
+- Default configuration: S0 = pen up (MIN_PULSE), S1000 = pen down (MAX_PULSE)
+- If your servo moves backwards, swap the MIN and MAX pulse values
+- Fine-tune the exact pulse values (16-31 range) to match your servo's physical limits

grbl/spindle_control.c

@@ -45,38 +45,22 @@ void spindle_init()
 
 void spindle_stop()
 {
-  #ifdef USE_SPINDLE_SERVO_MODE
+  // Disable PWM output (both servo and standard modes)
-    // Servo mode: M5 moves servo to maximum position (e.g., pen up) instead of disabling PWM
+  #ifdef VARIABLE_SPINDLE
-    // This keeps the servo powered and in position
+    TCCRA_REGISTER &= ~(1<<COMB_BIT); // Disable PWM. Output voltage is zero.
-    TCCRA_REGISTER = (1<<COMB_BIT) | (1<<WAVE1_REGISTER) | (1<<WAVE0_REGISTER);
-    TCCRB_REGISTER = (TCCRB_REGISTER & 0b11111000) | 0x07; // 1/1024 prescaler
-    OCR_REGISTER = SPINDLE_SERVO_MAX_PULSE; // Set to maximum position
-    
     #if defined(CPU_MAP_ATMEGA2560) || defined(USE_SPINDLE_DIR_AS_ENABLE_PIN)
-      #ifdef INVERT_SPINDLE_ENABLE_PIN
-        SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
-      #else
-        SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
-      #endif
-    #endif
-  #else
-    // Standard spindle mode: Disable PWM output
-    #ifdef VARIABLE_SPINDLE
-      TCCRA_REGISTER &= ~(1<<COMB_BIT); // Disable PWM. Output voltage is zero.
-      #if defined(CPU_MAP_ATMEGA2560) || defined(USE_SPINDLE_DIR_AS_ENABLE_PIN)
-        #ifdef INVERT_SPINDLE_ENABLE_PIN
-          SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
-        #else
-          SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
-        #endif
-      #endif
-    #else
       #ifdef INVERT_SPINDLE_ENABLE_PIN
         SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
       #else
         SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
       #endif
     #endif
+  #else
+    #ifdef INVERT_SPINDLE_ENABLE_PIN
+      SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
+    #else
+      SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
+    #endif
   #endif
 }
 
@@ -124,12 +108,15 @@ void spindle_set_state(uint8_t state, float rpm)
         else {
           // Servo mode: Map RPM range to servo pulse width (16-31 ticks = ~1-2ms pulses)
           // Each tick at 1/1024 prescaler = 64μs, so 16 ticks ≈ 1.024ms, 31 ticks ≈ 1.984ms
+          // INVERTED: S1000 = pen down (max pulse), S0 = pen up (min pulse)
           #define SPINDLE_SERVO_RANGE (SPINDLE_SERVO_MAX_PULSE - SPINDLE_SERVO_MIN_PULSE)
           #define SPINDLE_RPM_RANGE (SPINDLE_MAX_RPM-SPINDLE_MIN_RPM)
           if ( rpm < SPINDLE_MIN_RPM ) { rpm = SPINDLE_MIN_RPM; }
           if ( rpm > SPINDLE_MAX_RPM ) { rpm = SPINDLE_MAX_RPM; }
+          // Invert RPM mapping: S1000 → max pulse (pen down), S0 → min pulse (pen up)
+          rpm = SPINDLE_MAX_RPM - rpm;
           rpm -= SPINDLE_MIN_RPM;
-          // Map RPM to servo pulse range
+          // Map inverted RPM to servo pulse range
           current_pwm = floor( rpm * (SPINDLE_SERVO_RANGE / SPINDLE_RPM_RANGE) + SPINDLE_SERVO_MIN_PULSE + 0.5);
           OCR_REGISTER = current_pwm; // Set PWM pin output