Food dispenser WiPy food cycle with stuttering motion

main.py

# coding=utf-8
#
# Loop through food delivery cycle
#
import time
from machine import Pin

from servo import Servo

led = Pin('GP25', mode=Pin.OUT)
led(0)  # turn off the heartbeat LED

SERVO_WAIT = 50

# Servo specific constants
PULSE_MIN = 900  # in µs, actually bit higher than specs indicate
PULSE_MAX = 2100  # in µs
FREQUENCY = 50  # Hz
ROTATIONAL_RANGE_100 = 12000  # 120deg * 100

servo_top = Servo(10, FREQUENCY, ROTATIONAL_RANGE_100, PULSE_MIN, PULSE_MAX)
servo_bottom = Servo(9, FREQUENCY, ROTATIONAL_RANGE_100, PULSE_MIN, PULSE_MAX)

top_servo_limits = {
    'open': 0,  # deg
    'stutter': [75, 74, 73],  # deg
    'closed': 100,  # deg
    'direction': -1,  # angle direction from closed to open
}

bottom_servo_limits = {
    'open': 120,  # deg
    'stutter': [90, 100, 110],
    'closed': 20,  # deg
    'direction': 1,  # # angle direction from closed to open
}


def get_next_stutter(stutter_array):
    if (len(stutter_array) > 0):
        return stutter_array.pop()
    else:
        return None


def set_angle(servo, angle):
    servo.angle(angle * 100)
    time.sleep_ms(SERVO_WAIT)


def move_servo(servo, from_angle, to_angle, dir, stutter_array=[]):
    """
    Open or close servo, possibly shutters a bit to prevent jamming
    """
    stutter = get_next_stutter(stutter_array)

    range1 = range(from_angle, to_angle, dir)
    for angle in range1:
        set_angle(servo, angle)

        if angle == stutter:
            stutter_angles = range(angle + -15 * dir, angle, dir)
            for angle in stutter_angles:
                set_angle(servo, angle)
            stutter = get_next_stutter(stutter_array)


def cycle_servo(servo, limits):
    """
    Open and close servo
    """
    print('opening')
    move_servo(servo, limits['closed'], limits['open'], limits['direction'])

    print('closing')
    move_servo(servo, limits['open'], limits['closed'], limits['direction'] * -1, limits['stutter'])


cycle_servo(servo_top, top_servo_limits)
cycle_servo(servo_bottom, bottom_servo_limits)

led(1)  # turn on the heartbeat LED