Ensure Servo detach()es only on idle state

Avoid any short pulses which could cause servo twitches or damage by
adding a shutdown command to the PIO program and checking it's in that
safe part of the loop before detaching the servo.

Author: earlephilhower

libraries/Servo/src/Servo.cpp

@@ -24,10 +24,8 @@
 #include <hardware/clocks.h>
 #include <hardware/pio.h>
 
-// The PWM example from theSDK is general purpose enough to use here
-// with no PIO code changes whatsoever.  RP2040 is awesome!
-#include "pwm.pio.h"
-static PIOProgram _servoPgm(&pwm_program);
+#include "servo.pio.h"
+static PIOProgram _servoPgm(&servo_program);
 
 // similiar to map but will have increased accuracy that provides a more
 // symmetrical api (call it and use result to reverse will provide the original value)
@@ -80,21 +78,23 @@ int Servo::attach(pin_size_t pin, int minUs, int maxUs, int value)
     _minUs = max(200, min(_maxUs, minUs));
 
     if (!_attached) {
-        int off;
         digitalWrite(pin, LOW);
         pinMode(pin, OUTPUT);
-        if (!_servoPgm.prepare(&_pio, &_smIdx, &off)) {
+        _pin = pin;
+        if (!_servoPgm.prepare(&_pio, &_smIdx, &_pgmOffset)) {
             // ERROR, no free slots
             return -1;
         }
-        pwm_program_init(_pio, _smIdx, off, pin);
+	_attached = true;
+        servo_program_init(_pio, _smIdx, _pgmOffset, pin);
         pio_sm_set_enabled(_pio, _smIdx, false);
         pio_sm_put_blocking(_pio, _smIdx, RP2040::usToPIOCycles(REFRESH_INTERVAL) / 3);
         pio_sm_exec(_pio, _smIdx, pio_encode_pull(false, false));
         pio_sm_exec(_pio, _smIdx, pio_encode_out(pio_isr, 32));
+        write(value);
+        pio_sm_exec(_pio, _smIdx, pio_encode_pull(false, false));
+        pio_sm_exec(_pio, _smIdx, pio_encode_mov(pio_x, pio_osr));
         pio_sm_set_enabled(_pio, _smIdx, true);
-        _pin = pin;
-	_attached = true;
     }
 
     write(value);
@@ -105,6 +105,12 @@ int Servo::attach(pin_size_t pin, int minUs, int maxUs, int value)
 void Servo::detach()
 {
     if (_attached) {
+        // Set a 0 for the width and then wait for the halt loop
+        pio_sm_put_blocking(_pio, _smIdx, 0);
+        delayMicroseconds(5);  // Avoid race condition
+        do {
+            // Do nothing until we are stuck in the halt loop (avoid short pulses
+        } while (pio_sm_get_pc(_pio, _smIdx) != servo_offset_halt + _pgmOffset);
         pio_sm_set_enabled(_pio, _smIdx, false);
         pio_sm_unclaim(_pio, _smIdx);
         _attached = false;

libraries/Servo/src/pwm.pio.h

@@ -1,30 +1,38 @@
+; Servo.PIO - Generate Servo pulses
 ;
-; Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+; Based on PWM.PIO Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 ;
 ; SPDX-License-Identifier: BSD-3-Clause
 ;
 
 ; Side-set pin 0 is used for PWM output
 
-.program pwm
+; A servo count of 0 will cause an infinite loop allowing for
+; the RP2040 to determine when it is safe to shut down this
+; PIO program without causing any short pulses.
+; RP2040 code can check the PIO PC and determine when to stop it.
+
+.program servo
 .side_set 1 opt
 
     pull noblock    side 0 ; Pull from FIFO to OSR if available, else copy X to OSR.
     mov x, osr             ; Copy most-recently-pulled value back to scratch X
-    mov y, isr             ; ISR contains PWM period. Y used as counter.
+public halt:
+    jmp !x halt            ; If x == 0 then a halt was requested
+    mov y, isr             ; ISR contains servo period. Y used as counter.
 countloop:
     jmp x!=y noset         ; Set pin high if X == Y, keep the two paths length matched
     jmp skip        side 1
 noset:
     nop                    ; Single dummy cycle to keep the two paths the same length
 skip:
-    jmp y-- countloop      ; Loop until Y hits 0, then pull a fresh PWM value from FIFO
+    jmp y-- countloop      ; Loop until Y hits 0, then pull a fresh servo value from FIFO
 
 % c-sdk {
-static inline void pwm_program_init(PIO pio, uint sm, uint offset, uint pin) {
+static inline void servo_program_init(PIO pio, uint sm, uint offset, uint pin) {
    pio_gpio_init(pio, pin);
    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
-   pio_sm_config c = pwm_program_get_default_config(offset);
+   pio_sm_config c = servo_program_get_default_config(offset);
    sm_config_set_sideset_pins(&c, pin);
    pio_sm_init(pio, sm, offset, &c);
 }

libraries/Servo/src/pwm.pio renamed to libraries/Servo/src/servo.pio

@@ -0,0 +1,54 @@
+// -------------------------------------------------- //
+// This file is autogenerated by pioasm; do not edit! //
+// -------------------------------------------------- //
+
+#if !PICO_NO_HARDWARE
+#include "hardware/pio.h"
+#endif
+
+// ----- //
+// servo //
+// ----- //
+
+#define servo_wrap_target 0
+#define servo_wrap 7
+
+#define servo_offset_halt 2u
+
+static const uint16_t servo_program_instructions[] = {
+            //     .wrap_target
+    0x9080, //  0: pull   noblock         side 0     
+    0xa027, //  1: mov    x, osr                     
+    0x0022, //  2: jmp    !x, 2                      
+    0xa046, //  3: mov    y, isr                     
+    0x00a6, //  4: jmp    x != y, 6                  
+    0x1807, //  5: jmp    7               side 1     
+    0xa042, //  6: nop                               
+    0x0084, //  7: jmp    y--, 4                     
+            //     .wrap
+};
+
+#if !PICO_NO_HARDWARE
+static const struct pio_program servo_program = {
+    .instructions = servo_program_instructions,
+    .length = 8,
+    .origin = -1,
+};
+
+static inline pio_sm_config servo_program_get_default_config(uint offset) {
+    pio_sm_config c = pio_get_default_sm_config();
+    sm_config_set_wrap(&c, offset + servo_wrap_target, offset + servo_wrap);
+    sm_config_set_sideset(&c, 2, true, false);
+    return c;
+}
+
+static inline void servo_program_init(PIO pio, uint sm, uint offset, uint pin) {
+   pio_gpio_init(pio, pin);
+   pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
+   pio_sm_config c = servo_program_get_default_config(offset);
+   sm_config_set_sideset_pins(&c, pin);
+   pio_sm_init(pio, sm, offset, &c);
+}
+
+#endif
+