212 lines
7.9 KiB
Python
212 lines
7.9 KiB
Python
# 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
|