3.5. ServoSweep Arduino Sketch

This sketch is used by Exercise: Servo Sweep.

3.5.1. Full Source Code

The full code is all in one file ServoSweep.ino.

  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
// ServoSweep - move a servo along trajectories
//
// Copyright (c) 2016, Garth Zeglin.  All rights reserved. Licensed under the
// terms of the BSD 3-clause license as included in LICENSE.
//
// This program assumes that:
//
//  1. A small hobby servo is connected to pin 9.
//     Note: this sensor has +5V digital outputs can connect directly to the
//     digital input pins on the Arduino UNO.
//
//  2. The serial console on the Arduino IDE is set to 9600 baud communications speed.
//
// ================================================================================
// Import libraries.
#include <Servo.h> 

// ================================================================================
// Definitions of constant values.

// The wiring assignment.
const int SERVO_PIN = 9;

// ================================================================================
// Global variable declarations.

// Create an object to control the servo by declaring it.  The Servo C++ class
// is defined in the Servo library.
Servo wiggling_servo;

// Declaration for the subroutine function defined below.  Normally the Arduino system
// doesn't require forward declarations, but the optional argument defeats it.
void linear_move(int start, int end, float speed = 60.0);

// ================================================================================
// Configure the hardware once after booting up.  This runs once after pressing
// reset or powering up the board.

void setup()
{
  // Initialize the serial UART at 9600 bits per second.
  Serial.begin(9600);

  // Initialize the Servo object to use the given pin for output.
  wiggling_servo.attach(SERVO_PIN);
}

// ================================================================================
// Run one iteration of the main event loop.  The Arduino system will call this
// function over and over forever.
void loop()
{
  //================================================================
  // Movement template 1: perform several uncontrolled movements at maximum speed.
  for (int i = 0; i < 2; i = i+1) {
    wiggling_servo.write(0);
    delay(500);
    wiggling_servo.write(90);
    delay(500);
    wiggling_servo.write(180);
    delay(500);
    wiggling_servo.write(0);
    delay(500);
  }

  //================================================================
  // Movement template 2: loop over angles to create a series of stepped movements.
  for (int i = 0; i < 180; i += 10) {
    wiggling_servo.write(i);
    delay(500);
  }

  //================================================================
  // Movement template 3: sweep back and forth a few times using a subroutine
  // which produces a smooth sweep.
  for (int i = 0; i < 2; i = i+1) {
    // Call the movement function defined in the code below.
    linear_move(0, 180);
    linear_move(180, 0);
  }
  linear_move(0, 45); // move to the start point

  // Similar, but using different angles and a slower speed.
  for (int i = 0; i < 2; i = i+1) {
    linear_move(45, 135, 30);
    linear_move(135, 45, 30);
  }
  linear_move(45, 90);  // move to the center

  //================================================================
  // Movement template 4: generate a movement profile using a mathematical function.

  // Perform a smooth movement around the center several times.
  for (int i = 0; i < 4; i = i+1) {

    // Define a few constants governing the motion.  Note that this example
    // uses a C++ style of declaration which looks more like a normal variable
    // declaration, but whose value cannot be changed.
    const float center    = 90.0;     // in degrees
    const float magnitude = 30.0;     // in degrees
    const float period    =  4.0;     // in seconds, duration of cycle
    const float interval  =  0.020;   // in seconds, duration of each step

    int cycle_steps = period / interval;
    
    for (int step = 0; step < cycle_steps; step++) {
      // Compute the 'phase angle' for the sine function.  Note that the sin()
      // function requires an angle in radians.
      float phase = step * (2*M_PI/cycle_steps);
      
      // Compute the angle to send to the servo.
      float angle = center + magnitude * sin(phase);
      wiggling_servo.write(angle);

      // Wait for one sampling period.
      delay(1000*interval);
    }
  }
}
// ================================================================================
// Subroutine definitions.

// Linear servo movement function.  This will step from the start angle to the
// end angle as requested.  This emits servo updates at a constant rate.  It
// does not return until the movement is complete.
//
//   start - angle in degrees
//     end - angle in degrees
//   speed - optional argument, speed in degrees/sec
//   

void linear_move(int start, int end, float speed)
{
  // Specify the number of milliseconds to wait between updates.
  const int interval = 20;

  // Compute the size of each step in degrees.  Note the use of float to capture
  // fractional precision.  The constant converts speed units from milliseconds
  // to seconds:   deg/step = (deg/sec) * (sec/msec) * (msec/step)
  float step = speed * 0.001 * interval;
  
  // Declare a float variable to hold the current servo angle.
  float angle = start;

  // Begin a do-loop.  This always executes the body at least once, and then
  // iterates if the while condition is met.
  do {
    wiggling_servo.write(angle);  // update the servo output
    delay(interval);     // pause for the sampling interval

    if (end >= start) {
      angle += step;       // movement in the positive direction
      if (angle > end) angle = end;
    } else {
      angle -= step;      // movement in the negative direction
      if (angle < end) angle = end;
    }
  } while (angle != end);

  // Update the servo with the exact endpoint before returning.
  wiggling_servo.write(end);  
}
// ================================================================================