first commit servo commander code
This commit is contained in:
John Park
parent
bc5742ca2f
commit
749d43e635
1 changed files with 212 additions and 0 deletions
212
Servo_Commander/code.py
Normal file
212
Servo_Commander/code.py
Normal file
|
|
@ -0,0 +1,212 @@
|
|||
# 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
|
||||
Loading…
Reference in a new issue