Text Editor
Adafruit recommends using the Mu editor for editing your CircuitPython code. You can get more info in this guide.
Alternatively, you can use any text editor that saves simple text files.
Download the Project Bundle
Your project will use a specific set of CircuitPython libraries and the code.py file. To get everything you need, click on the Download Project Bundle link below, and uncompress the .zip file.
Drag the contents of the uncompressed bundle directory onto your board's CIRCUITPY drive, replacing any existing files or directories with the same names, and adding any new ones that are necessary.
# SPDX-FileCopyrightText: 2024 johnpark for Adafruit Industries
#
# SPDX-License-Identifier: MIT
'''
Servo Commander
- Feather Reverse TFT ESP32-S3 + two servos + two push encoders
- test servo ranges with encoder rotation
- store saved positions with enc button + D0, D1, D2 buttons
- recall saved positions with D0, D1, D2
- playback animation by pressing both push encoders
'''
import time
import board
import displayio
import terminalio
from adafruit_display_text import label
from adafruit_display_shapes.rect import Rect
import rotaryio
import pwmio
from adafruit_motor import servo
import keypad
# Define custom servo pulse range variables
s_cfgs = [
{'min_pulse': 600, 'max_pulse': 2400, 'min_ang': 10, 'max_ang': 170},
{'min_pulse': 600, 'max_pulse': 2300, 'min_ang': 10, 'max_ang': 170}
]
# Setup the PWM output for the servos
pwm_pins = [board.D10, board.D11]
servo_motors = []
for i, config in enumerate(s_cfgs):
pwm = pwmio.PWMOut(pwm_pins[i], duty_cycle=2**15, frequency=50)
servo_motor = servo.Servo(pwm, min_pulse=config['min_pulse'], max_pulse=config['max_pulse'])
servo_motors.append(servo_motor)
servo_motor.angle = 90
s_saves = [
[s_cfgs[0]['min_ang'], 90, s_cfgs[0]['max_ang']],
[s_cfgs[1]['min_ang'], 90, s_cfgs[1]['max_ang']]
]
# Setup the rotary encs
encs = [
rotaryio.IncrementalEncoder(board.A2, board.A1),
rotaryio.IncrementalEncoder(board.A5, board.A4)
]
for enc in encs:
enc.position = 90
last_positions = [enc.position for enc in encs]
# Setup the buttons
enc_buttons = keypad.Keys((board.D13, board.D12), value_when_pressed=False, pull=True)
tft_d0_button = keypad.Keys((board.D0,), value_when_pressed=False, pull=True)
tft_buttons = keypad.Keys((board.D1, board.D2), value_when_pressed=True, pull=True)
def set_servo_angle(s_servo_motor, s_angle, s_enc, min_ang, max_ang):
s_angle = min(max(s_angle, min_ang), max_ang)
s_servo_motor.angle = s_angle
s_enc.position = s_angle
def playback(mode, speed, steps):
for k in range(3): # Loop through each save position (assuming 3 saves per motor)
for m in range(len(servo_motors)):
p_servo_motor = servo_motors[m]
p_enc = encs[m]
save = s_saves[m][k] # Get the k-th save position for the m-th motor
if mode:
direction = 1 if save > enc.position else -1
for p_angle in range(enc.position, save, direction * steps):
p_servo_motor.angle = p_angle
time.sleep(speed)
p_enc.position = save
else:
servo_motor.angle = save
time.sleep(0.75)
# Setup the display
display = board.DISPLAY
group = displayio.Group()
background_rect = Rect(0, 10, display.width, display.height - 10, fill=0x000010)
group.append(background_rect)
mid_bar = Rect(116, 0, 3, display.height, fill=0x00000)
group.append(mid_bar)
top_bar = Rect(0, 0, display.width, 20, fill=0x000000)
group.append(top_bar)
FONT = terminalio.FONT
TXTCOL = 0xFFFF00
# Create labels
labels = []
for i, config in enumerate(s_cfgs):
lbl = label.Label(FONT, text=f"Pulse: {config['min_pulse']}-{config['max_pulse']}",color=TXTCOL,
scale=1, anchor_point=(0, 0), anchored_position=(5 + 125 * i, 5))
labels.append(lbl)
group.append(lbl)
for i in range(2):
lbl = label.Label(FONT, text="Angle:-", color=TXTCOL, scale=2, anchor_point=(0, 0),
anchored_position=(4 + 126 * i, 24))
labels.append(lbl)
group.append(lbl)
for i in range(2):
for j in range(3):
lbl = label.Label(FONT, text=f"D{j}:{s_saves[i][j]}", color=TXTCOL, scale=2,
anchor_point=(0, 0), anchored_position=(4 + i * 126, 48 + 24 * j))
labels.append(lbl)
group.append(lbl)
display.root_group = group
modifier1 = False
modifier2 = False
print("[]-Servo Commander READY-[]")
while True:
enc_button_event = enc_buttons.events.get()
if enc_button_event:
if enc_button_event.pressed:
if enc_button_event.key_number == 0:
modifier1 = True
elif enc_button_event.key_number == 1:
modifier2 = True
if enc_button_event.released:
if enc_button_event.key_number == 0:
modifier1 = False
elif enc_button_event.key_number == 1:
modifier2 = False
if modifier1 and modifier2:
print("Playback")
playback(True, 0.006, 2)
tft_d0_button_event = tft_d0_button.events.get()
if tft_d0_button_event and tft_d0_button_event.pressed:
if modifier1:
s_saves[0][0] = min(max(encs[0].position, s_cfgs[0]['min_ang']), s_cfgs[0]['max_ang'])
print("D0 save motor1:", s_saves[0][0])
labels[4].text = f"D0:{s_saves[0][0]}"
elif modifier2:
s_saves[1][0] = min(max(encs[1].position, s_cfgs[1]['min_ang']), s_cfgs[1]['max_ang'])
print("D0 save motor2:", s_saves[1][0])
labels[7].text = f"D0:{s_saves[1][0]}"
else:
for i in range(len(servo_motors)):
servo_motor = servo_motors[i]
enc = encs[i]
s_save = s_saves[i][0]
config = s_cfgs[i]
print(f"D0 recalled motor{i+1}:", s_save)
set_servo_angle(servo_motor, s_save, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{s_save}"
tft_buttons_event = tft_buttons.events.get()
if tft_buttons_event and tft_buttons_event.pressed:
if tft_buttons_event.key_number == 0:
if modifier1:
s_saves[0][1] = min(max(encs[0].position,s_cfgs[0]['min_ang']),s_cfgs[0]['max_ang'])
print("D1 save motor1:", s_saves[0][1])
labels[5].text = f"D1:{s_saves[0][1]}"
elif modifier2:
s_saves[1][1] = min(max(encs[1].position,s_cfgs[1]['min_ang']),s_cfgs[1]['max_ang'])
print("D1 save motor2:", s_saves[1][1])
labels[8].text = f"D1:{s_saves[1][1]}"
else:
for i in range(len(servo_motors)):
servo_motor = servo_motors[i]
enc = encs[i]
s_save = s_saves[i][1]
config = s_cfgs[i]
print(f"D1 recalled motor{i+1}:", s_save)
set_servo_angle(servo_motor, s_save, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{s_save}"
elif tft_buttons_event.key_number == 1:
if modifier1:
s_saves[0][2] = min(max(encs[0].position,s_cfgs[0]['min_ang']),s_cfgs[0]['max_ang'])
print("D2 save motor1:", s_saves[0][2])
labels[6].text = f"D2:{s_saves[0][2]}"
elif modifier2:
s_saves[1][2] = min(max(encs[1].position,s_cfgs[1]['min_ang']),s_cfgs[1]['max_ang'])
print("D2 save motor2:", s_saves[1][2])
labels[9].text = f"D2:{s_saves[1][2]}"
else:
for i in range(len(servo_motors)):
servo_motor = servo_motors[i]
enc = encs[i]
s_save = s_saves[i][2]
config = s_cfgs[i]
print(f"D2 recalled motor{i+1}:", s_save)
set_servo_angle(servo_motor, s_save, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{s_save}"
for i in range(len(servo_motors)):
current_position = encs[i].position
if current_position != last_positions[i]:
config = s_cfgs[i]
angle = min(max(current_position, config['min_ang']), config['max_ang'])
servo_motor = servo_motors[i]
enc = encs[i]
set_servo_angle(servo_motor, angle, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{angle}"
last_positions[i] = current_position
How It Works
The Servo Boss uses a Feather Reverse TFT ESP32-S2 to control two servo motors using two rotary encoders and several buttons. You can use it to test servo ranges with encoder rotation, store positions using button presses, recall these positions, and play back stored animations.
Setup
Libraries
First, you'll import the required libraries, including displayio related libraries, rotaryio for encoder reading, and pwmio and adafruit_motor for driving servos.
import time import board import displayio import terminalio from adafruit_display_text import label from adafruit_display_shapes.rect import Rect import rotaryio import pwmio from adafruit_motor import servo import keypad
Servo Configuration
Begin by setting up the servo motors. Two servos are initialized with custom pulse ranges, which define their minimum and maximum pulse widths. The servos are connected to pins D10 and D11:
s_cfgs = [
{'min_pulse': 600, 'max_pulse': 2400, 'min_ang': 10, 'max_ang': 170},
{'min_pulse': 600, 'max_pulse': 2300, 'min_ang': 10, 'max_ang': 170}
]
Each servo is configured using the pwmio and adafruit_motor libraries, and their initial angles are set to 90 degrees:
pwm_pins = [board.D10, board.D11]
servo_motors = []
for i, config in enumerate(s_cfgs):
pwm = pwmio.PWMOut(pwm_pins[i], duty_cycle=2**15, frequency=50)
servo_motor = servo.Servo(pwm, min_pulse=config['min_pulse'], max_pulse=config['max_pulse'])
servo_motors.append(servo_motor)
servo_motor.angle = 90
Rotary Encoders
Two rotary encoders are set up to control the angles of the servos. The encoders are connected to pins D10 and D11 and then initialized with a starting position of 90 degrees:
encs = [
rotaryio.IncrementalEncoder(board.A2, board.A1),
rotaryio.IncrementalEncoder(board.A5, board.A4)
]
for enc in encs:
enc.position = 90
Button Configuration
Several buttons are configured to save and recall positions. Two encoder buttons and the three buttons (D0, D1, D2) on the TFT Feather are set up using the keypad library:
enc_buttons = keypad.Keys((board.D13, board.D12), value_when_pressed=False, pull=True) tft_d0_button = keypad.Keys((board.D0,), value_when_pressed=False, pull=True) tft_buttons = keypad.Keys((board.D1, board.D2), value_when_pressed=True, pull=True)
Display Setup
The display is initialized to show the servo pulse ranges, current angles, and saved positions:
display = board.DISPLAY group = displayio.Group() background_rect = Rect(0, 10, display.width, display.height - 10, fill=0x000010) group.append(background_rect) # Additional display elements are added here display.root_group = group
Main Loop
The main loop checks for button presses and encoder changes in order to control the servos and update the display:
Button Presses
- Single Encoder Buttons: When either encoder button is pressed along with one of the TFT buttons, the current position for the relative servo is stored in that save slot.
- Both Encoder Buttons: When both encoder buttons are pressed, the playback function is triggered to play back stored animations.
- D0, D1, D2 Buttons: When any of these are pressed on their own, both servo motors go to their saved positions for that save slot. When pressed at the same time as an encoder button saves the current position.
while True:
enc_button_event = enc_buttons.events.get()
if enc_button_event:
if enc_button_event.pressed:
if enc_button_event.key_number == 0:
modifier1 = True
elif enc_button_event.key_number == 1:
modifier2 = True
if enc_button_event.released:
if enc_button_event.key_number == 0:
modifier1 = False
elif enc_button_event.key_number == 1:
modifier2 = False
if modifier1 and modifier2:
print("Playback")
playback(True, 0.006, 2)
tft_d0_button_event = tft_d0_button.events.get()
if tft_d0_button_event and tft_d0_button_event.pressed:
if modifier1:
s_saves[0][0] = min(max(encs[0].position, s_cfgs[0]['min_ang']), s_cfgs[0]['max_ang'])
print("D0 save motor1:", s_saves[0][0])
labels[4].text = f"D0:{s_saves[0][0]}"
elif modifier2:
s_saves[1][0] = min(max(encs[1].position, s_cfgs[1]['min_ang']), s_cfgs[1]['max_ang'])
print("D0 save motor2:", s_saves[1][0])
labels[7].text = f"D0:{s_saves[1][0]}"
else:
for i in range(len(servo_motors)):
servo_motor = servo_motors[i]
enc = encs[i]
s_save = s_saves[i][0]
config = s_cfgs[i]
print(f"D0 recalled motor{i+1}:", s_save)
set_servo_angle(servo_motor, s_save, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{s_save}"
tft_buttons_event = tft_buttons.events.get()
if tft_buttons_event and tft_buttons_event.pressed:
if tft_buttons_event.key_number == 0:
if modifier1:
s_saves[0][1] = min(max(encs[0].position,s_cfgs[0]['min_ang']),s_cfgs[0]['max_ang'])
print("D1 save motor1:", s_saves[0][1])
labels[5].text = f"D1:{s_saves[0][1]}"
elif modifier2:
s_saves[1][1] = min(max(encs[1].position,s_cfgs[1]['min_ang']),s_cfgs[1]['max_ang'])
print("D1 save motor2:", s_saves[1][1])
labels[8].text = f"D1:{s_saves[1][1]}"
else:
for i in range(len(servo_motors)):
servo_motor = servo_motors[i]
enc = encs[i]
s_save = s_saves[i][1]
config = s_cfgs[i]
print(f"D1 recalled motor{i+1}:", s_save)
set_servo_angle(servo_motor, s_save, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{s_save}"
elif tft_buttons_event.key_number == 1:
if modifier1:
s_saves[0][2] = min(max(encs[0].position,s_cfgs[0]['min_ang']),s_cfgs[0]['max_ang'])
print("D2 save motor1:", s_saves[0][2])
labels[6].text = f"D2:{s_saves[0][2]}"
elif modifier2:
s_saves[1][2] = min(max(encs[1].position,s_cfgs[1]['min_ang']),s_cfgs[1]['max_ang'])
print("D2 save motor2:", s_saves[1][2])
labels[9].text = f"D2:{s_saves[1][2]}"
else:
for i in range(len(servo_motors)):
servo_motor = servo_motors[i]
enc = encs[i]
s_save = s_saves[i][2]
config = s_cfgs[i]
print(f"D2 recalled motor{i+1}:", s_save)
set_servo_angle(servo_motor, s_save, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{s_save}"
Direct Servo Movements
When you turn either rotary encoder, the corresponding servo angle is updated, while clamping the encoder values in the defined range:
for i in range(len(servo_motors)):
current_position = encs[i].position
if current_position != last_positions[i]:
config = s_cfgs[i]
angle = min(max(current_position, config['min_ang']), config['max_ang'])
servo_motor = servo_motors[i]
enc = encs[i]
set_servo_angle(servo_motor, angle, enc, config['min_ang'], config['max_ang'])
labels[2 + i].text = f"Angle:{angle}"
last_positions[i] = current_position
Playback Function
The playback function animates the servos through saved positions, either smoothly or instantly depending on the mode:
def playback(mode, speed, steps):
for k in range(3):
for m in range(len(servo_motors)):
p_servo_motor = servo_motors[m]
p_enc = encs[m]
save = s_saves[m][k]
if mode:
direction = 1 if save > enc.position else -1
for p_angle in range(enc.position, save, direction * steps):
p_servo_motor.angle = p_angle
time.sleep(speed)
p_enc.position = save
else:
servo_motor.angle = save
time.sleep(0.75)
Page last edited January 22, 2025
Text editor powered by tinymce.
