1  # SPDX-FileCopyrightText: 2018 Dave Astels for Adafruit Industries
  2  #
  3  # SPDX-License-Identifier: MIT
  4  
  5  """
  6  Servo Tester
  7  
  8  Adafruit invests time and resources providing this open source code.
  9  Please support Adafruit and open source hardware by purchasing
 10  products from Adafruit!
 11  
 12  Written by Dave Astels for Adafruit Industries
 13  Copyright (c) 2018 Adafruit Industries
 14  Licensed under the MIT license.
 15  
 16  All text above must be included in any redistribution.
 17  """
 18  
 19  import time
 20  import board
 21  import busio
 22  import digitalio
 23  import rotaryio
 24  import pwmio
 25  import adafruit_ssd1306
 26  from adafruit_motor import servo
 27  from adafruit_debouncer import Debouncer
 28  
 29  #--------------------------------------------------------------------------------
 30  # Initialize Rotary encoder
 31  
 32  button_io = digitalio.DigitalInOut(board.D12)
 33  button_io.direction = digitalio.Direction.INPUT
 34  button_io.pull = digitalio.Pull.UP
 35  button = Debouncer(button_io)
 36  rotary_encoder = rotaryio.IncrementalEncoder(board.D10, board.D11)
 37  
 38  #--------------------------------------------------------------------------------
 39  # Initialize I2C and OLED
 40  
 41  i2c = busio.I2C(board.SCL, board.SDA)
 42  
 43  oled = adafruit_ssd1306.SSD1306_I2C(128, 32, i2c)
 44  oled.fill(0)
 45  oled.show()
 46  
 47  min_pulses = [ 500,  550,  600,  650,  700,  750,  800,  850,  900,  950, 1000]
 48  max_pulses = [2000, 2050, 2100, 2150, 2200, 2250, 2300, 2350, 2400, 2450, 2500]
 49  
 50  min_pulse_index = 10
 51  max_pulse_index = 0
 52  
 53  #-------------------------------------------------------------------------------
 54  # Initialize servo
 55  
 56  pwm = pwmio.PWMOut(board.D5, frequency=50)
 57  test_servo = servo.Servo(pwm, min_pulse=1000, max_pulse=2000)
 58  test_servo.angle = 0
 59  
 60  current_position = None               # current encoder position
 61  change = 0                            # the change in encoder position
 62  angle = 0
 63  mode = 0
 64  sweep_time = 1.0
 65  last_movement_at = 0.0
 66  delta = 5
 67  
 68  
 69  def get_encoder_change(encoder, pos):
 70      new_position = encoder.position
 71      if pos is None:
 72          return (new_position, 0)
 73      else:
 74          return (new_position, new_position - pos)
 75  
 76  #--------------------------------------------------------------------------------
 77  # Main loop
 78  
 79  while True:
 80      now = time.monotonic()
 81      button.update()
 82  
 83      if mode == 1:
 84          if now >= (last_movement_at + sweep_time / 36):
 85              last_movement_at = now
 86              angle += delta
 87              if (angle > 180) or (angle < 0):
 88                  delta *= -1
 89                  angle += delta
 90  
 91      if button.fell:
 92          servo.angle = 0
 93          if mode == 0:
 94              mode = 1
 95              sweep_time = 1.0
 96              last_movement_at = now
 97          elif mode == 1:
 98              mode = 2
 99              angle = 0
100          elif mode == 2:
101              mode = 3
102              angle = 180
103          elif mode == 3:
104              mode = 0
105              angle = 0
106  
107      else:
108          current_position, change = get_encoder_change(rotary_encoder, current_position)
109          if change != 0:
110              if mode == 0:
111                  angle = min(180, max(0, angle + change * 5))
112              elif mode == 1:
113                  sweep_time = min(5.0, max(1.0, sweep_time + change * 0.1))
114              elif mode == 2:
115                  min_pulse_index = min(10, max(min_pulse_index + change, 0))
116                  test_servo = servo.Servo(pwm,
117                                           min_pulse=min_pulses[min_pulse_index],
118                                           max_pulse=max_pulses[max_pulse_index])
119                  angle = 0
120              elif mode == 3:
121                  max_pulse_index = min(10, max(max_pulse_index + change, 0))
122                  test_servo = servo.Servo(pwm,
123                                           min_pulse=min_pulses[min_pulse_index],
124                                           max_pulse=max_pulses[max_pulse_index])
125                  angle = 180
126  
127      oled.fill(0)
128      if mode == 0:
129          oled.text("Angle: {0}".format(angle), 0, 0)
130      elif mode == 1:
131          oled.text("Sweep time: {0}".format(sweep_time), 0, 0)
132      elif mode == 2:
133          oled.text("Min width: {0}".format(min_pulses[min_pulse_index]), 0, 0)
134      elif mode == 3:
135          oled.text("Max width: {0}".format(max_pulses[max_pulse_index]), 0, 0)
136      oled.show()
137  
138      test_servo.angle = angle