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// ================================================================================