Skip to content

Commit

Permalink
Completed servo examples
Browse files Browse the repository at this point in the history
ZodiusInfuser committed Oct 13, 2023
1 parent b54def2 commit 88cba07
Showing 7 changed files with 302 additions and 154 deletions.
100 changes: 100 additions & 0 deletions examples/modules/quad_servo/all_servos.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
import math
from pimoroni_yukon import Yukon
from pimoroni_yukon.modules import QuadServoDirectModule, QuadServoRegModule
from pimoroni_yukon.timing import ticks_ms, ticks_add
from pimoroni_yukon.logging import LOG_WARN
from servo import ServoCluster

"""
How to drive up to 24 servos from a set of Quad Servo Modules connected to Slots, using a ServoCluster.
A wave pattern will be played on the attached servos.
The ServoCluster controls the whole set of servos using PIO.
It also staggers the updates of each servo to reduce peak current draw.
"""

# Constants
SPEED = 0.005 # How much to advance the servo phase offset by each update
UPDATES = 50 # How many times to update the servos per second
SERVO_EXTENT = 80.0 # How far from zero to move the servos
START_DELAY = 0.5 # The time to sleep between activating and animating the servos
CLUSTER_PIO = 0 # The PIO system to use (0 or 1) to drive the servo cluster
CLUSTER_SM = 0 # The State Machines (SM) to use to drive the servo cluster

# Variables
yukon = Yukon(logging_level=LOG_WARN) # Create a new Yukon object, with its logging level lowered
modules = [] # A list to store QuadServo module objects created later
phase_offset = 0 # The offset used to animate the servos


# Function to get a servo angle from its index
def angle_from_index(index, offset=0.0):
phase = ((index / NUM_SERVOS) + offset) * math.pi * 2
angle = math.sin(phase) * SERVO_EXTENT
return angle


# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt)
try:
# Find out which slots of Yukon have QuadServoDirectModules attached
for slot in yukon.find_slots_with(QuadServoDirectModule):
module = QuadServoDirectModule(init_servos=False) # Create a QuadServoDirectModule object, but do not have it create Servo objects
yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot
modules.append(module) # Add the object to the module list

# Find out which slots of Yukon have QuadServoRegModule attached
for slot in yukon.find_slots_with(QuadServoRegModule):
module = QuadServoRegModule(init_servos=False) # Create a QuadServoRegModule object, but do not have it create Servo objects
yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot
modules.append(module) # Add the object to the module list

# Record the number of servos that will be driven
NUM_SERVOS = len(modules) * QuadServoDirectModule.NUM_SERVOS
print(f"Up to {NUM_SERVOS} servos available")

yukon.verify_and_initialise() # Verify that QuadServo modules are attached to Yukon, and initialise them

# Create a ServoCluster object, with a list of servo pins to control.
# The pin list is created using nested list comprehension
servos = ServoCluster(CLUSTER_PIO, CLUSTER_SM,
pins=[pin for module in modules for pin in module.servo_pins])

yukon.enable_main_output() # Turn on power to the module slots

for module in modules:
if hasattr(module, 'enable'):
module.enable() # Enable each QuadServoRegModule's onboard regulator

# Move all servos to their starting positions
for current_servo in range(servos.count()):
servos.value(current_servo, angle_from_index(current_servo))
yukon.monitored_sleep(START_DELAY) # 'Sleep' for a short time for each servo to reach their position
# This avoids them all drawing peak current at once

current_time = ticks_ms() # Record the start time of the program loop

# Loop until the BOOT/USER button is pressed
while not yukon.is_boot_pressed():

# Move all the servos to new values
for current_servo in range(servos.count()):
angle = angle_from_index(current_servo, phase_offset)
servos.value(current_servo, angle)

# Advance the phase offset, wrapping if it exceeds 1.0
phase_offset += SPEED
if phase_offset >= 1.0:
phase_offset -= 1.0

print(f"Phase = {phase_offset}")

# Advance the current time by a number of seconds
current_time = ticks_add(current_time, int(1000 / UPDATES))

# Monitor sensors until the current time is reached, recording the min, max, and average for each
# This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete
yukon.monitor_until_ms(current_time)

finally:
# Put the board back into a safe state, regardless of how the program may have ended
yukon.reset()
81 changes: 81 additions & 0 deletions examples/modules/quad_servo/four_servos.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
import math
from pimoroni_yukon import Yukon
from pimoroni_yukon import SLOT1 as SLOT

# Choose one of these two lines to uncomment, depending on the module used
from pimoroni_yukon.modules import QuadServoRegModule as QuadServoModule
# from pimoroni_yukon.modules import QuadServoDirectModule as QuadServoModule

from pimoroni_yukon.timing import ticks_ms, ticks_add
from pimoroni_yukon.logging import LOG_WARN

"""
How to drive up to four servos from a Quad Servo Module connected to Slot1.
A wave pattern will be played on the attached servos.
"""

# Constants
SPEED = 0.005 # How much to advance the servo phase offset by each update
UPDATES = 50 # How many times to update the servos per second
SERVO_EXTENT = 80.0 # How far from zero to move the servos
START_DELAY = 0.5 # The time to sleep between activating and animating the servos

# Variables
yukon = Yukon(logging_level=LOG_WARN) # Create a new Yukon object, with its logging level lowered
module = QuadServoModule() # Create a QuadServoModule object
phase_offset = 0 # The offset used to animate the servos


# Function to get a servo angle from its index
def angle_from_index(index, offset=0.0):
phase = ((index / QuadServoModule.NUM_SERVOS) + offset) * math.pi * 2
angle = math.sin(phase) * SERVO_EXTENT
return angle


# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt)
try:
yukon.register_with_slot(module, SLOT) # Register the QuadServoModule object with the slot
yukon.verify_and_initialise() # Verify that a QuadServoModule is attached to Yukon, and initialise it
yukon.enable_main_output() # Turn on power to the module slots

if hasattr(module, 'enable'):
module.enable() # If module is a QuadServoRegulatedModule, enable its onboard regulator

# Move all servos to their starting positions
current_servo = 0
for servo in module.servos:
servo.value(angle_from_index(current_servo))
yukon.monitored_sleep(START_DELAY) # 'Sleep' for a short time for each servo to reach their position
# This avoids them all drawing peak current at once
current_servo += 1

current_time = ticks_ms() # Record the start time of the program loop

# Loop until the BOOT/USER button is pressed
while not yukon.is_boot_pressed():

# Move all the servos to new values
current_servo = 0
for servo in module.servos:
angle = angle_from_index(current_servo, phase_offset)
servo.value(angle)
current_servo += 1

# Advance the phase offset, wrapping if it exceeds 1.0
phase_offset += SPEED
if phase_offset >= 1.0:
phase_offset -= 1.0

print(f"Phase = {phase_offset}")

# Advance the current time by a number of seconds
current_time = ticks_add(current_time, int(1000 / UPDATES))

# Monitor sensors until the current time is reached, recording the min, max, and average for each
# This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete
yukon.monitor_until_ms(current_time)

finally:
# Put the board back into a safe state, regardless of how the program may have ended
yukon.reset()
96 changes: 96 additions & 0 deletions examples/modules/quad_servo/multiple_servos.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
import math
from pimoroni_yukon import Yukon
from pimoroni_yukon.modules import QuadServoDirectModule, QuadServoRegModule
from pimoroni_yukon.timing import ticks_ms, ticks_add
from pimoroni_yukon.logging import LOG_WARN

"""
How to drive up to 16 servos from a set of Quad Servo Modules connected to Slots.
A wave pattern will be played on the attached servos.
To use more servos, or to reduce peak current draw, look at the all_servos.py example.
"""

# Constants
SPEED = 0.005 # How much to advance the servo phase offset by each update
UPDATES = 50 # How many times to update the servos per second
SERVO_EXTENT = 80.0 # How far from zero to move the servos
START_DELAY = 0.5 # The time to sleep between activating and animating the servos

# Variables
yukon = Yukon(logging_level=LOG_WARN) # Create a new Yukon object, with its logging level lowered
modules = [] # A list to store QuadServo module objects created later
phase_offset = 0 # The offset used to animate the servos


# Function to get a servo angle from its index
def angle_from_index(index, offset=0.0):
phase = ((index / NUM_SERVOS) + offset) * math.pi * 2
angle = math.sin(phase) * SERVO_EXTENT
return angle


# Wrap the code in a try block, to catch any exceptions (including KeyboardInterrupt)
try:
# Find out which slots of Yukon have QuadServoDirectModules attached
for slot in yukon.find_slots_with(QuadServoDirectModule):
module = QuadServoDirectModule() # Create a QuadServoDirectModule object
yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot
modules.append(module) # Add the object to the module list

# Find out which slots of Yukon have QuadServoRegModule attached
for slot in yukon.find_slots_with(QuadServoRegModule):
module = QuadServoRegModule() # Create a QuadServoRegModule object
yukon.register_with_slot(module, slot) # Register the QuadServoDirectModule object with the slot
modules.append(module) # Add the object to the module list

# Record the number of servos that will be driven
NUM_SERVOS = len(modules) * QuadServoDirectModule.NUM_SERVOS
print(f"Up to {NUM_SERVOS} servos available")

yukon.verify_and_initialise() # Verify that QuadServo modules are attached to Yukon, and initialise them
yukon.enable_main_output() # Turn on power to the module slots

for module in modules:
if hasattr(module, 'enable'):
module.enable() # Enable each QuadServoRegModule's onboard regulator

# Move all servos to their starting positions
current_servo = 0
for module in modules:
for servo in module.servos:
servo.value(angle_from_index(current_servo))
yukon.monitored_sleep(START_DELAY) # 'Sleep' for a short time for each servo to reach their position
# This avoids them all drawing peak current at once
current_servo += 1

current_time = ticks_ms() # Record the start time of the program loop

# Loop until the BOOT/USER button is pressed
while not yukon.is_boot_pressed():

# Move all the servos to new values
current_servo = 0
for module in modules:
for servo in module.servos:
angle = angle_from_index(current_servo, phase_offset)
servo.value(angle)
current_servo += 1

# Advance the phase offset, wrapping if it exceeds 1.0
phase_offset += SPEED
if phase_offset >= 1.0:
phase_offset -= 1.0

print(f"Phase = {phase_offset}")

# Advance the current time by a number of seconds
current_time = ticks_add(current_time, int(1000 / UPDATES))

# Monitor sensors until the current time is reached, recording the min, max, and average for each
# This approach accounts for the updating of the rainbows taking a non-zero amount of time to complete
yukon.monitor_until_ms(current_time)

finally:
# Put the board back into a safe state, regardless of how the program may have ended
yukon.reset()
61 changes: 0 additions & 61 deletions examples/yukon_adaptive_led_strips.py

This file was deleted.

Loading

0 comments on commit 88cba07

Please sign in to comment.