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);
}
}
}
|