# servo_sweep.py # # Raspberry Pi Pico - hobby servo motion demo # # Demonstrates smoothly moving a hobby servo back and forth. # # This assumes a tiny 9G servo has been wired up to the Pico as follows: # Pico pin 40 (VBUS) -> servo red (+5V) # Pico pin 38 (GND) -> servo brown (GND) # Pico pin 1 (GP0) -> servo orange (SIG) # links to CircuitPython module documentation: # time https://circuitpython.readthedocs.io/en/latest/shared-bindings/time/index.html # math https://circuitpython.readthedocs.io/en/latest/shared-bindings/math/index.html # board https://circuitpython.readthedocs.io/en/latest/shared-bindings/board/index.html # pwmio https://circuitpython.readthedocs.io/en/latest/shared-bindings/pwmio/index.html ################################################################################ # print a banner as reminder of what code is loaded print("Starting servo_sweep script.") # load standard Python modules import math, time # load the CircuitPython hardware definition module for pin definitions import board # load the CircuitPython pulse-width-modulation module for driving hardware import pwmio #-------------------------------------------------------------------------------- # Class to represent a single hardware hobby servo. This wraps up all the # configuration information and current state into a single object. The # creation of the object also initializes the physical pin hardware. class Servo(): def __init__(self, pin, pulse_rate=50): # Create a PWMOut object on the desired pin to drive the servo. # Note that the initial duty cycle of zero generates no pulses, which # for many servos will present as a quiescent low-power state. self.pwm = pwmio.PWMOut(board.GP0, duty_cycle=0, frequency=pulse_rate) # Save the initialization arguments within the object for later reference. self.pin = pin self.pulse_rate = pulse_rate # Initialize the other state variables. self.target = None # target angle; None indicates the "OFF" state self.debug = False # flag to control print output def write(self, angle): # Object method to issue a servo command by updating the PWM signal # output. This function maps an angle specified in degrees between 0 # and 180 to a servo command pulse width between 1 and 2 milliseconds, # and then to the corresponding duty cycle fraction, specified as a # 16-bit fixed-point integer. As a special case, an angle value of None # will turn off the output; many servos will then become backdrivable. # calculate the desired pulse width in units of seconds if angle is None: pulse_width = 0.0 else: pulse_width = 0.001 + angle * (0.001 / 180.0) # calculate the duration in seconds of a single pulse cycle cycle_period = 1.0 / self.pulse_rate # calculate the desired ratio of pulse ON time to cycle duration duty_cycle = pulse_width / cycle_period # convert the ratio into a 16-bit fixed point integer duty_fixed = int(2**16 * duty_cycle) # limit the ratio range and apply to the hardware driver self.pwm.duty_cycle = min(max(duty_fixed, 0), 65535) # save the target value in the object attribute (i.e. variable) self.target = angle # if requested, print some diagnostics to the console if self.debug: print(f"Driving servo to angle {angle}") print(f" Pulse width {pulse_width} seconds") print(f" Duty cycle {duty_cycle}") print(f" Command value {self.pwm.duty_cycle}\n") #-------------------------------------------------------------------------------- # Movement primitive to smoothly move from a start to end angle at a constant rate. # It does not return until the movement is complete. # The angles are specified in degrees, the speed in degrees/second. def linear_move(servo, start, end, speed=60, update_rate=50): # Calculate the number of seconds to wait between target updates to allow # the motor to move. # Units: seconds = 1.0 / (cycles/second) interval = 1.0 / update_rate # Compute the size of each step in degrees. # Units: degrees = (degrees/second) * second step = speed * interval # Output the start angle once before beginning the loop. This guarantees at # least one angle will be output even if the start and end are equal. angle = start servo.write(angle) # Loop once for each incremental angle change. while angle != end: time.sleep(interval) # pause for the sampling interval # Update the target angle. The positive and negative movement directions # are treated separately. if end >= start: angle += step; # movement in the positive direction if angle > end: angle = end # end at an exact position else: angle -= step # movement in the negative direction if angle < end: angle = end # end at an exact position servo.write(angle) # update the hardware #-------------------------------------------------------------------------------- # Movement primitive to generate a smooth oscillating movement by simulating a # spring-mass-damper system. It does not return until the movement is complete. # This is an example of simple harmonic motion and uses a differential equation # to specify a motion implicitly. # The q_d parameter specifies the center angle of the oscillation, conceptually # is the angle of the simulated spring anchor point. # The default parameters were selected as follows: # q = 0.0 initial position # qd = 0.0 initial velocity # k = 4*pi*pi spring constant for 1 Hz: freq = (1/2*pi) * sqrt(k/m); k = (freq*2*pi)^2 # b = 2.0 damping constant def ringing_move(servo, q_d, q=0.0, qd=0.0, k=4*math.pi*math.pi, b=2.0, update_rate=50, duration=2.0, debug=False): # Calculate the number of seconds to wait between target updates to allow # the motor to move. # Units: seconds = 1.0 / (cycles/second) interval = 1.0 / update_rate while duration > 0.0: # Calculate the acceleration. # qdd acceleration in angles/sec^2 # k spring constant relating the acceleration to the angular displacement # b damping constant relating the acceleration to velocity qdd = k * (q_d - q) - b * qd # integrate one time step using forward Euler integration q += qd * interval # position changes in proportion to velocity qd += qdd * interval # velocity changes in proportion to acceleration # update the servo command with the new angle servo.write(q) # print the output for plotting if requested if debug: print(q) # Delay to control timing. This is an inexact strategy, since it doesn't account for # any execution time of this function. time.sleep(interval) duration -= interval #-------------------------------------------------------------------------------- # Create an object to represent a servo on the given hardware pin. print("Creating servo object.") servo = Servo(board.GP0) # Enable (voluminous) debugging output. # servo.debug = True # Begin the main processing loop. This is structured as a looping script, since # each movement primitive 'blocks', i.e. doesn't return until the action is # finished. print("Starting main script.") while True: # initial pause time.sleep(2.0) # begin the movement sequence, starting with some slow sweeps print("Starting linear motions.") linear_move(servo, 0.0, 180.0, speed=45) linear_move(servo, 180.0, 0.0, speed=22.5) # brief pause; stillness is the counterpoint time.sleep(1.0) # start bouncy oscillation movements print("Starting ringing motions.") ringing_move(servo, 90.0, duration=1.5) ringing_move(servo, 45.0, duration=1.5) ringing_move(servo, 90.0, duration=1.5) # Final oscillation into a a grand pause at the end, but never actually # stopping; stillness isn't necessarily stationary. This also initializes # the generator to begin the movement at the target angle, but with positive # velocity. print("Starting final ringing motion.") ringing_move(servo, 90.0, q=90.0, qd=400, b=1.0, duration=12.0)