Clear PIO FIFOs when updating Tone and Servos (#206)

The PIO programs that generate tone() and Servo() use the TX FIFO to
receive updates to the period/duty cycle.

The original code would push into the FIFO (potentially blocking the
app if the FIFO was full) and generate at least one cycle of every
value written into the control.  Basically, the output would
lag the changes by 1 cycle or more (which could be 20ms+ on Servo).

Fix this by clearing any old, ungrabbed values from the FIFO before
sending a new one to the program.  Instead of a FIFO, there is
effectively now just a control register and updates will be immediate.

Update the Siren.ino example with delays because now the tone() calls
will not block and run 10x+ faster.

Author: earlephilhower

cores/rp2040/Tone.cpp

@@ -88,6 +88,7 @@ void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
             newTone->alarm = 0;
         }
     }
+    pio_sm_clear_fifos(newTone->pio, newTone->sm); // Remove any old updates that haven't yet taken effect
     pio_sm_put_blocking(newTone->pio, newTone->sm, RP2040::usToPIOCycles(us));
     pio_sm_set_enabled(newTone->pio, newTone->sm, true);
 

libraries/Servo/src/Servo.cpp

@@ -135,6 +135,7 @@ void Servo::writeMicroseconds(int value) {
     value = constrain(value, _minUs, _maxUs);
     _valueUs = value;
     if (_attached) {
+        pio_sm_clear_fifos(_pio, _smIdx); // Remove any old updates that haven't yet taken effect
         pio_sm_put_blocking(_pio, _smIdx, RP2040::usToPIOCycles(value) / 3);
     }
 }

libraries/rp2040/examples/Siren/Siren.ino

@@ -7,7 +7,8 @@ void setup() {
 }
 
 void loop() {
-  for (int i = 100; i < 10000; i += 5) {
+  for (int i = 100; i < 10000; i += 1) {
     tone(TONEPIN, i);
+    delayMicroseconds(20);
   }
 }