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

// ================================================================================
// Define constant values and global variables.

// The wiring assignment.
#define SERVO_PIN 9

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

// ================================================================================
// 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.
  servo.attach(SERVO_PIN);
}
// ================================================================================
// 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 = 60.0)
{
  // 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 {
    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.
  servo.write(end);  
}

// ================================================================================
// Run one iteration of the main event loop.  The Arduino system will call this
// function over and over forever.
void loop()
{
  // Perform several uncontrolled movements at maximum speed.
  for (int i = 0; i < 2; i++) {
    servo.write(0);
    delay(500);
    servo.write(90);
    delay(500);
    servo.write(180);
    delay(500);
    servo.write(0);
    delay(500);
  }
  
  // Sweep back and forth a few times.
  for (int i = 0; i < 2; i++) {
    linear_move(0, 180);
    linear_move(180, 0);
  }

  linear_move(0, 45); // move to the start point
  
  // Sweep back and forth slowly a few times.
  for (int i = 0; i < 2; i++) {
    linear_move(45, 135, 30);
    linear_move(135, 45, 30);
  }
  
  linear_move(45, 90);  // move to the center
  
  // Perform a smooth movement around the center several times.
  for (int i = 0; i < 4; i++) {

    // 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);
      servo.write(angle);

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