Day 2: (Wed Sep 1, Week 1) Hobby Servo Motion

Notes for 2021-09-01.

New Assignments

  1. New assignment, due Wed Sep 8: Exercise: Servo Motion with the Pico

  2. Before the next class, please arrange access to SolidWorks 2021-2022 (the version now in the clusters). Here are several options:

    1. by installing on your personal Windows laptop (notes)

    2. by installing in personal Windows virtual machine (on any platform)

    3. via the https://virtual.andrew.cmu.edu/ Engineering cluster

    4. by borrowing an IDeATe Windows laptop

  3. If you are new to SolidWorks 3D modeling, I highly recommend you go through the beginner tutorial available via CMU LinkedIn Learning (formerly Lynda). (Note that it does appear possible to use this without a LinkedIn account.) Here’s a starting point: Learning SOLIDWORKS. (two hours).

Administrative

  1. Please note that Monday September 6 is Labor Day, no class.

  2. Please test your ID card on the door lock and let me know if it doesn’t work.

  3. Reminder: please stay on top of laser cutter qualification. We’re working on opening up more training sessions.

Agenda

  1. Introduction to hobby servos.

  2. Programming movement trajectories.

  3. Soldering tutorial.

  4. Hands-on experimentation.

Figures

../_images/Micro-9g-Servo-For-RC-Helicopter-Boat-Plane-Car.jpg

Micro-size hobby servo, a feedback-controlled device with integral driver.

Lecture code samples

See also Hobby Servo Examples - Raspberry Pi Pico.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
# servo_step.py
#
# Raspberry Pi Pico - hobby servo motion demo
#
# Demonstrates stepping 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

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

#--------------------------------------------------------------------------------
# Define a function 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.
# The optional pulse_rate argument specifies the pulse repetition rate in Hz
# (pulses per second).

def servo_write(servo, angle, pulse_rate=50, debug=False):
    # calculate the desired pulse width in units of seconds
    pulse_width  = 0.001 + angle * (0.001 / 180.0)

    # calculate the duration in seconds of a single pulse cycle
    cycle_period = 1.0 / 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
    servo.duty_cycle = min(max(duty_fixed, 0), 65535)

    # print some diagnostics to the console
    if 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 {servo.duty_cycle}\n")

#--------------------------------------------------------------------------------
# Create a PWMOut object on Pin GP0 to drive the servo.
servo = pwmio.PWMOut(board.GP0, duty_cycle=0, frequency=50)

# Begin the main processing loop.
while True:
    servo_write(servo, 0.0, debug=True)
    time.sleep(2.0)

    servo_write(servo, 180.0, debug=True)
    time.sleep(2.0)

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
# 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)