# Servo alignment program using RP2040 state machines
# Developed to adjust Robi robot's servos
# Robi uses a siple Fet level shifter, so PWM signal is vreated inverted
# Hardware is a RP2040 pico or picoW board.
#
# PIO program is defined in module pio_pwm, which is imported by this module
# A PIO PWM class is defined in module pio_pwm, which is used below
# start program from REPL with >>> import servo_alignment
# 
# initial revision 23-Dec-2022 by wolf2018
#
#
import pio_pwm
import gc

# PIO PWM constants.
#  Adjust these constants according to your servo specifications.
#  This setup is for standard servo with:
#  20ms cycle time and 1ms to 2ms pulse width for a 0 to 90 degree rotation
#
pio_freq = 800000
pwm_count = 8000
init_value = -1    # disables PWM on instantiation to avoid movement
servo_min = pwm_count - 400
servo_max = pwm_count - 800
servo_range = 400

instructions = """
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Instructions for using the servo alignment program on RP2040

Servos connect to GPIO Pins 0 to 7 using a simple FET level shifter

Servo number and the GPIO number correspond:
    Servo 0 is connected to GPIO0 (Pin 1 on the RP2040 pico board)
    Servo 1 is connected to GPIO1 (Pin 2 on the RP2040 pico board)
    Servo 2 is connected to GPIO2 (Pin 4 of the RP2040 pico board)
    ...

The state machine (sm) used corresponds to the servo number
Servo 0 uses sm 0, servo 1 uses sm 1, ...

    ----------------------------------------------------
    ! Be aware that Wlan on the picoW board uses sm 4, !
    ! so do not initialize WLan when running this test !
    ----------------------------------------------------

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

"""


def calc_pwm_count(s_angle):
    """ convert degree to pwm_count; input is servo angle"""

    p_count = int(servo_range * (s_angle/90) + servo_max)
    return p_count


def run_test(servo):
    # servo test runs as long as a number is entered
    gc.collect()

    while 1:
        # ask for servo angle, exit test if a character is entered by the user
        try:
            angle = input("\nenter servo angle [0...90], 'E' to exit test: ")
            s_angle = float(angle)

            if s_angle < 0 or s_angle > 90:
                print("\n-----> servo angle needs to be between 0 and 90 degree")

            else:
                pw = calc_pwm_count(s_angle)
                servo.set(pw)

        except ValueError:
            print("you entered '", angle, "'exiting servo test")
            servo.de_init()
            break

        except KeyboardInterrupt:
            servo.de_init()
            print("\ngot Ctrl_C, de-init state machine")
            print("exiting servo test")
            break


def main():
    print(instructions)
    try:
        while True:

            gc.collect()
            print("To exit the program with press Ctrl-C")
            servo_nbr = input("\nPlease enter a servo number [0..7]: ")

            try:
                s_nbr = int(servo_nbr)
                #print("s_nbr is of type: ", type(s_nbr))
                if s_nbr in range(0, 8):
                    # instantiate the servo
                    print("initializing state machine for servo: ", str(s_nbr))
                    servo = pio_pwm.PIOPWM(s_nbr, s_nbr, pwm_count, pio_freq, init_value)
                    run_test(servo)
                else:
                    print("\nservo number is not between 0 and 7")
                    pass

            except ValueError:
                print("\nplease enter a number between 0 and 7")

    except KeyboardInterrupt:
        print("\n\ngot Ctrl-C, ending the program")


# start the program
main()