# module to use PIO for PWM to control the servo angle
# this is based on the PIO_PWM example of the Pico_SDK documnetation
#
# initial rev 24-Apr-2022 by wolf2018
#
# updates:
# 6-Jul-2022: add de_init function to stop state machines
# 19-Dec-2022: disable PWM by default (init_value=-1) to avoid servo jitter until a valid signal is generated

from machine import Pin
from rp2 import PIO, StateMachine, asm_pio

# PIO PWM uses 2 cycles per pwm count
# y holds the pwm count which determins the pwm period
# x holds the pwm count, which determins the active time  (high or low) at the beginning of the period
# For Robi robot hardware GPIO output is inverted (active low) to accomodate for a Fet level shifter
# if you need a non-inverted output signal, swap the logical levels for the side directives (see comments in @asm_pio)


@asm_pio(sideset_init=PIO.OUT_LOW)
def pwm_prog():
    pull(noblock) .side(0)  # set side() to 1 for non-inverted output
    mov(x, osr)             # Keep most recent pull data stored in X, for recycling by noblock
    mov(y, isr)             # ISR must be preloaded with PWM count max
    label("pwmloop")
    jmp(x_not_y, "skip")
    nop()   .side(1)        # set side() to 0 for non-inverted output
    label("skip")
    jmp(y_dec, "pwmloop")


class PIOPWM:

    def __init__(self, sm_id, pin, max_count, count_freq, init_value=-1):
        #
        self._sm = StateMachine(sm_id, pwm_prog, freq=count_freq, sideset_base=Pin(pin))
        # use exec() to load max count into ISR
        self._sm.put(max_count)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        # if no init_value is given, pwm is disabled by default
        self._sm.put(init_value)
        self._sm.active(1)

    def set(self, value):
        # Minimum value is -1 (completely turn off),
        # 0 actually still produces narrow pulse and may move the servo to its end position
        self._sm.put(value)

    def de_init(self):
        """ de-init state machines to disable PWM output"""
        self._sm.active(0)