ServoSweep Arduino Sketch¶
This sketch is used by Exercise: Servo Sweep.
Full Source Code¶
The full code is all in one file ServoSweep.ino.
1// ServoSweep - move a servo along trajectories
2//
3// Copyright (c) 2016, Garth Zeglin. All rights reserved. Licensed under the
4// terms of the BSD 3-clause license as included in LICENSE.
5//
6// This program assumes that:
7//
8// 1. A small hobby servo is connected to pin 9.
9// Note: this sensor has +5V digital outputs can connect directly to the
10// digital input pins on the Arduino UNO.
11//
12// 2. The serial console on the Arduino IDE is set to 9600 baud communications speed.
13//
14// ================================================================================
15// Import libraries.
16#include <Servo.h>
17
18// ================================================================================
19// Definitions of constant values.
20
21// The wiring assignment.
22const int SERVO_PIN = 9;
23
24// ================================================================================
25// Global variable declarations.
26
27// Create an object to control the servo by declaring it. The Servo C++ class
28// is defined in the Servo library.
29Servo wiggling_servo;
30
31// Declaration for the subroutine function defined below. Normally the Arduino system
32// doesn't require forward declarations, but the optional argument defeats it.
33void linear_move(int start, int end, float speed = 60.0);
34
35// ================================================================================
36// Configure the hardware once after booting up. This runs once after pressing
37// reset or powering up the board.
38
39void setup()
40{
41 // Initialize the serial UART at 9600 bits per second.
42 Serial.begin(9600);
43
44 // Initialize the Servo object to use the given pin for output.
45 wiggling_servo.attach(SERVO_PIN);
46}
47
48// ================================================================================
49// Run one iteration of the main event loop. The Arduino system will call this
50// function over and over forever.
51void loop()
52{
53 //================================================================
54 // Movement template 1: perform several uncontrolled movements at maximum speed.
55 for (int i = 0; i < 2; i = i+1) {
56 wiggling_servo.write(0);
57 delay(500);
58 wiggling_servo.write(90);
59 delay(500);
60 wiggling_servo.write(180);
61 delay(500);
62 wiggling_servo.write(0);
63 delay(500);
64 }
65
66 //================================================================
67 // Movement template 2: loop over angles to create a series of stepped movements.
68 for (int i = 0; i < 180; i += 10) {
69 wiggling_servo.write(i);
70 delay(500);
71 }
72
73 //================================================================
74 // Movement template 3: sweep back and forth a few times using a subroutine
75 // which produces a smooth sweep.
76 for (int i = 0; i < 2; i = i+1) {
77 // Call the movement function defined in the code below.
78 linear_move(0, 180);
79 linear_move(180, 0);
80 }
81 linear_move(0, 45); // move to the start point
82
83 // Similar, but using different angles and a slower speed.
84 for (int i = 0; i < 2; i = i+1) {
85 linear_move(45, 135, 30);
86 linear_move(135, 45, 30);
87 }
88 linear_move(45, 90); // move to the center
89
90 //================================================================
91 // Movement template 4: generate a movement profile using a mathematical function.
92
93 // Perform a smooth movement around the center several times.
94 for (int i = 0; i < 4; i = i+1) {
95
96 // Define a few constants governing the motion. Note that this example
97 // uses a C++ style of declaration which looks more like a normal variable
98 // declaration, but whose value cannot be changed.
99 const float center = 90.0; // in degrees
100 const float magnitude = 30.0; // in degrees
101 const float period = 4.0; // in seconds, duration of cycle
102 const float interval = 0.020; // in seconds, duration of each step
103
104 int cycle_steps = period / interval;
105
106 for (int step = 0; step < cycle_steps; step++) {
107 // Compute the 'phase angle' for the sine function. Note that the sin()
108 // function requires an angle in radians.
109 float phase = step * (2*M_PI/cycle_steps);
110
111 // Compute the angle to send to the servo.
112 float angle = center + magnitude * sin(phase);
113 wiggling_servo.write(angle);
114
115 // Wait for one sampling period.
116 delay(1000*interval);
117 }
118 }
119}
120// ================================================================================
121// Subroutine definitions.
122
123// Linear servo movement function. This will step from the start angle to the
124// end angle as requested. This emits servo updates at a constant rate. It
125// does not return until the movement is complete.
126//
127// start - angle in degrees
128// end - angle in degrees
129// speed - optional argument, speed in degrees/sec
130//
131
132void linear_move(int start, int end, float speed)
133{
134 // Specify the number of milliseconds to wait between updates.
135 const int interval = 20;
136
137 // Compute the size of each step in degrees. Note the use of float to capture
138 // fractional precision. The constant converts speed units from milliseconds
139 // to seconds: deg/step = (deg/sec) * (sec/msec) * (msec/step)
140 float step = speed * 0.001 * interval;
141
142 // Declare a float variable to hold the current servo angle.
143 float angle = start;
144
145 // Begin a do-loop. This always executes the body at least once, and then
146 // iterates if the while condition is met.
147 do {
148 wiggling_servo.write(angle); // update the servo output
149 delay(interval); // pause for the sampling interval
150
151 if (end >= start) {
152 angle += step; // movement in the positive direction
153 if (angle > end) angle = end;
154 } else {
155 angle -= step; // movement in the negative direction
156 if (angle < end) angle = end;
157 }
158 } while (angle != end);
159
160 // Update the servo with the exact endpoint before returning.
161 wiggling_servo.write(end);
162}
163// ================================================================================