From a28e0fb6aabd0779c50c6989a9f25bc3c9216e2e Mon Sep 17 00:00:00 2001 From: Dave Astels Date: Wed, 14 Nov 2018 13:59:25 -0500 Subject: [PATCH] Code for Servo tester guide --- Servo_Tester/debouncer.py | 92 +++++++++++++++++++++++++++ Servo_Tester/main.py | 128 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 220 insertions(+) create mode 100644 Servo_Tester/debouncer.py create mode 100644 Servo_Tester/main.py diff --git a/Servo_Tester/debouncer.py b/Servo_Tester/debouncer.py new file mode 100644 index 000000000..1da07ac07 --- /dev/null +++ b/Servo_Tester/debouncer.py @@ -0,0 +1,92 @@ +""" +GPIO Pin Debouncer + +Adafruit invests time and resources providing this open source code. +Please support Adafruit and open source hardware by purchasing +products from Adafruit! + +Written by Dave Astels for Adafruit Industries +Copyright (c) 2018 Adafruit Industries +Licensed under the MIT license. + +All text above must be included in any redistribution. +""" + +import time +import digitalio + +class Debouncer(object): + """Debounce an input pin""" + + DEBOUNCED_STATE = 0x01 + UNSTABLE_STATE = 0x02 + CHANGED_STATE = 0x04 + + + def __init__(self, pin, mode=None, interval=0.010): + """Make am instance. + :param int pin: the pin (from board) to debounce + :param int mode: digitalio.Pull.UP or .DOWN (default is no pull up/down) + :param int interval: bounce threshold in seconds (default is 0.010, i.e. 10 milliseconds) + """ + self.state = 0x00 + self.pin = digitalio.DigitalInOut(pin) + self.pin.direction = digitalio.Direction.INPUT + if mode != None: + self.pin.pull = mode + if self.pin.value: + self.__set_state(Debouncer.DEBOUNCED_STATE | Debouncer.UNSTABLE_STATE) + self.previous_time = 0 + if interval is None: + self.interval = 0.010 + else: + self.interval = interval + + + def __set_state(self, bits): + self.state |= bits + + + def __unset_state(self, bits): + self.state &= ~bits + + + def __toggle_state(self, bits): + self.state ^= bits + + + def __get_state(self, bits): + return (self.state & bits) != 0 + + + def update(self): + """Update the debouncer state. Must be called before using any of the properties below""" + self.__unset_state(Debouncer.CHANGED_STATE) + current_state = self.pin.value + if current_state != self.__get_state(Debouncer.UNSTABLE_STATE): + self.previous_time = time.monotonic() + self.__toggle_state(Debouncer.UNSTABLE_STATE) + else: + if time.monotonic() - self.previous_time >= self.interval: + if current_state != self.__get_state(Debouncer.DEBOUNCED_STATE): + self.previous_time = time.monotonic() + self.__toggle_state(Debouncer.DEBOUNCED_STATE) + self.__set_state(Debouncer.CHANGED_STATE) + + + @property + def value(self): + """Return the current debounced value of the input.""" + return self.__get_state(Debouncer.DEBOUNCED_STATE) + + + @property + def rose(self): + """Return whether the debounced input went from low to high at the most recent update.""" + return self.__get_state(self.DEBOUNCED_STATE) and self.__get_state(self.CHANGED_STATE) + + + @property + def fell(self): + """Return whether the debounced input went from high to low at the most recent update.""" + return (not self.__get_state(self.DEBOUNCED_STATE)) and self.__get_state(self.CHANGED_STATE) diff --git a/Servo_Tester/main.py b/Servo_Tester/main.py new file mode 100644 index 000000000..990956c7a --- /dev/null +++ b/Servo_Tester/main.py @@ -0,0 +1,128 @@ +""" +Servo Tester + +Adafruit invests time and resources providing this open source code. +Please support Adafruit and open source hardware by purchasing +products from Adafruit! + +Written by Dave Astels for Adafruit Industries +Copyright (c) 2018 Adafruit Industries +Licensed under the MIT license. + +All text above must be included in any redistribution. +""" + +import time +import board +import busio +import digitalio +import rotaryio +import pulseio +import adafruit_ssd1306 +from adafruit_motor import servo + +from debouncer import Debouncer + +#-------------------------------------------------------------------------------- +# Initialize Rotary encoder + +button = Debouncer(board.D12, digitalio.Pull.UP, 0.01) +encoder = rotaryio.IncrementalEncoder(board.D10, board.D11) + +#-------------------------------------------------------------------------------- +# Initialize I2C and OLED + +i2c = busio.I2C(board.SCL, board.SDA) + +oled = adafruit_ssd1306.SSD1306_I2C(128, 32, i2c) +oled.fill(0) +oled.show() + +min_pulses = [ 500, 550, 600, 650, 700, 750, 800, 850, 900, 950, 1000] +max_pulses = [2000, 2050, 2100, 2150, 2200, 2250, 2300, 2350, 2400, 2450, 2500] + +min_pulse_index = 10 +max_pulse_index = 0 + +#------------------------------------------------------------------------------- +# Initialize servo + +pwm = pulseio.PWMOut(board.D5, frequency=50) +test_servo = servo.Servo(pwm, min_pulse=500, max_pulse=2500) +test_servo.angle = 0 + +current_position = None # current encoder position +change = 0 # the change in encoder position +angle = 0 +mode = 0 +sweep_time = 1.0 +last_movement_at = 0.0 +delta = 5 + + +def get_encoder_change(encoder, pos): + new_position = encoder.position + if pos is None: + return (new_position, 0) + else: + return (new_position, new_position - pos) + +#-------------------------------------------------------------------------------- +# Main loop + +while True: + now = time.monotonic() + button.update() + + if mode == 1: + if now >= (last_movement_at + sweep_time / 36): + last_movement_at = now + angle += delta + if (angle > 180) or (angle < 0): + delta *= -1 + angle += delta + + if button.fell: + servo.angle = 0 + if mode == 0: + mode = 1 + sweep_time = 1.0 + last_movement_at = now + elif mode == 1: + mode = 2 + angle = 0 + elif mode == 2: + mode = 3 + angle = 180 + elif mode == 3: + mode = 0 + angle = 0 + + else: + current_position, change = get_encoder_change(encoder, current_position) + if change != 0: + if mode == 0: + angle = min(180, max(0, angle + change * 5)) + elif mode == 1: + sweep_time = min(5.0, max(1.0, sweep_time + change * 0.1)) + elif mode == 2: + min_pulse_index = min(10, max(min_pulse_index + change, 0)) + test_servo = servo.Servo(pwm, min_pulse=min_pulses[min_pulse_index], max_pulse=max_pulses[max_pulse_index]) + angle = 0 + elif mode == 3: + max_pulse_index = min(10, max(max_pulse_index + change, 0)) + test_servo = servo.Servo(pwm, min_pulse=min_pulses[min_pulse_index], max_pulse=max_pulses[max_pulse_index]) + angle = 180 + + oled.fill(0) + if mode == 0: + oled.text("Angle: {0}".format(angle), 0, 0) + elif mode == 1: + oled.text("Sweep time: {0}".format(sweep_time), 0, 0) + elif mode == 2: + oled.text("Min width: {0}".format(min_pulses[min_pulse_index]), 0, 0) + elif mode == 3: + oled.text("Max width: {0}".format(max_pulses[max_pulse_index]), 0, 0) + oled.show() + + test_servo.angle = angle