Merge pull request #2839 from jedgarpark/servo-commander

first commit servo commander code

Author: TheKitty

Servo_Commander/code.py

@@ -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