testrig.py

#!/usr/bin/env python3

import signal
import time
import RPi.GPIO as gpio


BLINK_DUR   = 1  # seconds

# Reference for PINS:
# https://i.sstatic.net/yHddo.png
PIN_BUTTON  = 7
PIN_GREEN   = 40 
PIN_SERVO   = 37 
PIN_YELLOW  = 38 


def main(servo_pwm):
    blink_on, blink_ts = False, 0 

    while True:
        pressed = not gpio.input(PIN_BUTTON)
        if pressed:
            gpio.output(PIN_GREEN, gpio.HIGH)
            gpio.output(PIN_YELLOW, gpio.HIGH)
            test_loop(servo_pwm)

            gpio.output(PIN_GREEN, gpio.LOW)
            gpio.output(PIN_YELLOW, gpio.LOW)
            blink_on, blink_ts = False, time.time()
        elif time.time() - blink_ts > BLINK_DUR:
            gpio.output(PIN_YELLOW, gpio.LOW if blink_on else gpio.HIGH)
            blink_on, blink_ts = not blink_on, time.time()

        time.sleep(0.05)  # 50ms => 20hz.


def test_loop(servo_pwm):
    #print(f"Test loop running at {time.time()}")
    servo_pwm.ChangeDutyCycle(7.5)
    time.sleep(1)
    servo_pwm.ChangeDutyCycle(2.5)


def init_gpio():
    gpio.setmode(gpio.BOARD)
    gpio.setwarnings(False)

    gpio.setup(PIN_BUTTON,  gpio.IN, pull_up_down=gpio.PUD_UP)
    gpio.setup(PIN_GREEN,   gpio.OUT)
    gpio.setup(PIN_SERVO,   gpio.OUT)
    gpio.setup(PIN_YELLOW,  gpio.OUT)

    gpio.output(PIN_GREEN,  gpio.LOW)
    gpio.output(PIN_YELLOW, gpio.LOW)

    servo_pwm = gpio.PWM(PIN_SERVO, 50)
    servo_pwm.start(0)  # neutral position.
    return servo_pwm


if __name__ == "__main__":
    # Catch SIGTERM (e.g. from OpenRC) and cleanup GPIO first.
    signal.signal(signal.SIGTERM, lambda sig, frame: gpio.cleanup())

    try:
        servo_pwm = init_gpio()
        main(servo_pwm)
    except KeyboardInterrupt:
        print("Shutting down!")
        servo_pwm.stop()
        gpio.cleanup()