ServoTester
The following Arduino code demonstrates two functions for controlling an RC servo.
Most RC servos move an arm a fixed number of degrees based on the pulse width provide to the servo. Typical pulse widths are from 1 to 2 msec, One and two msec being the extremes, 0 and 180 degrees and 1.5 msec the mid-point. There is variations in all servos. RC transmitters have adjustments or the midpoint to center aircraft control surfaces, as well as limits for the endpoints.
I used this code to evaluate three types of servos: Tower Hobby, SG90, an old standard size Futaba and a Dymond D60.
This code uses the Arduino serial monitor to accept commands entered through a laptop to exercise one or more servos. It calls one of two functions. One function generates a single pulse of specified time (usec) to a specified servo. There is an array the maps servo, 0...N, to pin. The second function calls the first to generate a sequence of pulses that slowly rotates a specified servo to a specified position (usec). There is also a debug option to continually generate a pulse to a specified servo.
The serial monitor command sets a target time (usec) for a servo. Each iteration of loop() invokes servoRotate() which adjusts the pulse duration in the desired direction and invokes servoPulse() to generate the pulse. ServoRotate() compares the current position (usec) to a target to determine if a pulse needs to be generated. Mulitple servos can be active at the same time.
The top of the file specifies version string which is output at startup to the serial monitor to identify the code. It also specifies and enumeration for OK and ERROR symbols. Without explicitly specifying values (e.g. =3), values are sequentially assigned starting from zero. There are two debug constants and a debug flag.
// ServoFuncs - routines to pulse and slowly rotate servo
//
// sg90 500-2400 us
// 4.8V
// 25 oz-in
// 0.12 sec/60 deg
// plastic
// JR connector orange/red/black pwm, Vcc, Gnd
const char version [] = "ServoFuncs 0925c";
enum { OK, ERROR };
#define DEBUG_PRNT 1
#define DEBUG_CONT 2
int debug = 1;
// ---------------------------------------------------------
The next section defines constants used by this servo code. There are limits for the pulse widths used to test time values specified in commands. There is a mid-point calculated from the limits. Delta specifies how much the time is modified with each iteration when rotating the servo. ServoTimeNull is the time between pulses. Both Delta and ServoTimeNull affect how slowly the servo rotates.
The values are used to initialize similarly named variables (constants start with upper-case and variables with lower-case), allowing these values to be changed by a serial monitor command.
// constants
#define ServoTimeMin 300
#define ServoTimeMax 2400
#define ServoTimeMid ((ServoTimeMax - ServoTimeMin)/2)
#define Delta 10
#define ServoTimeDwell 20 // msec, time between pulses
int delta = Delta;
int servoTimeDwell = ServoTimeDwell;
// ---------------------------------------------------------
The servoPins [] maps servos, 0, 1, ... to pins. ServoTime [] maintains the position (usec) of each servo. The serial monitor commands do not specify a servo. There is a command to select the servo that a commands apply to and the servo that is optionally repeatedly pulsed. TimeUsec is the last pulse duration specified and used as the duration when a servo is repeatedly pulse.
// arrays indentifying pins for N_SERVOS
#define N_SERVOS 2
int servoPins [N_SERVOS] = { 5, 6 };
int servoTime [N_SERVOS] = { ServoTimeMid, ServoTimeMid };
int servoTarg [N_SERVOS] = { ServoTimeMid, ServoTimeMid };
int servo = 0; // current servo
int timeUsec = 1000; // last pulse duration
// ---------------------------------------------------------
The servoPulse () generates a single pulse of specified time for the specified servo. It first maps the servo to a pin. It does check if the pulse duration is within limits, generating an error message and returning. There is an optional debug print indicating the parameters.
Generating the pulse is straight forward. Set the pin HIGH, wait the specified pulse duration and set the pin LOW. There is an additional delay so that the routine returns after a total of 20 msec. And the pulse duration is stored in timeUsec.
// generate usec pulse for specified servo/pin
int
servoPulse (
int servo,
int usec )
{
int pin = servoPins [servo];
if (usec < ServoTimeMin || ServoTimeMax < usec) {
Serial.print ("servoPulse: invalid pulse duration (usec) ");
Serial.println (usec);
return ERROR;
}
if (DEBUG_PRNT & debug) {
Serial.print ("servoPulse: ");
Serial.print (servo);
Serial.print (" servo, ");
Serial.print (pin);
Serial.print (" pin, ");
Serial.print (usec);
Serial.println (" usec");
}
digitalWrite (pin, HIGH);
delayMicroseconds (usec);
digitalWrite (pin, LOW);
timeUsec = usec;
return OK;
}
// ---------------------------------------------------------
ServoRotate() repeatedly calls servoTime(), adjusting the duration maintain in servoTime[] each iteration until the desired pulse duration has been generated. There is a redundant test for limits, performed in servoTime(). An optional debug print can display parameters.
// slowly rotate servo from current to new position (usec)
void
servoRotate (
int servo )
{
int usec = servoTarg [servo];
int pin = servoPins [servo];
if (usec == servoTime [servo])
return;
if (DEBUG_PRNT & debug) {
Serial.print ("servoRotate: ");
Serial.print (servo);
Serial.print (" servo, ");
Serial.print (pin);
Serial.print (" pin ");
Serial.print (servoTime [servo]);
Serial.print (" time");
Serial.print (servoTarg [servo]);
Serial.println (" targ ");
}
servoTime [servo] += servoTime [servo] < usec ? delta : -delta;
servoPulse (servo, servoTime [servo]);
}
// -----------------------------------------------------------------------------
pcRead() reads command strings from the serial monitor. Commands are entered somewhat backwards. To specify a time (e.g. 1200 usec), each digit is separately processed and used to update the val variable. A command letter (e.g. 'r') is then recognized and the corresponding function called with arguments (e.g. servoRotate (servo, val). Val is then reset to zero for the next command. Other commands set the debug flag or other variables. The 's' command checks if the servo number if valid. The '?' command prints a description of the commands.
// process single character commands from the PC
void
pcRead (void)
{
static int val = 0;
if (Serial.available()) {
int c = Serial.read ();
switch (c) {
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
val = (10 * val) + c - '0';
Serial.print ("val: ");
Serial.println (val);
break;
case 'C':
debug ^= DEBUG_CONT;
break;
case 'D':
debug ^= 1;
break;
case 'd':
delta = val;
val = 0;
break;
case 'n':
servoTimeDwell = val;
val = 0;
break;
case 'r':
servoTarg [servo] = val;
val = 0;
break;
case 's':
if (N_SERVOS > val) {
servo = val;
Serial.print ("pcRead: servo ");
Serial.println (servo);
}
else {
Serial.print ("pcRead: servo must be less than ");
Serial.println (N_SERVOS);
}
val = 0;
break;
case 'p':
servoPulse (servo, val);
val = 0;
break;
case 'v':
Serial.print ("\nversion: ");
Serial.println (version);
break;
case 'z':
val = 0;
break;
case '?':
Serial.println (" C toggle continuous pulse flag");
Serial.println (" D toggle debug print flag");
Serial.println (" ##d set servo time (usec) step size");
Serial.println (" ##n set servo update (usec) period");
Serial.println (" ##r rotate servo to specified usec");
Serial.println (" ##s set servo #");
Serial.println (" ##p pulse servo to specified usec");
Serial.println (" v version");
Serial.println (" ? this screen");
break;
default:
break;
}
}
}
// -----------------------------------------------------------------------------
The Arduino loop() simply calls pcRead(). Each command is run to completion before a new command can be entered. If the debug flag is set for repeatedly generating pulse, servoPulse() is called for the currently selected servo and the last pulse duration.
void
loop (void)
{
static unsigned long lastMsec = 0;
unsigned long msec = millis();
pcRead (); // check for PC input
if (DEBUG_CONT & debug)
servoPulse (servo, timeUsec);
else if (servoTimeDwell < (msec - lastMsec)) {
for (int i = 0; i < N_SERVOS; i++)
servoRotate (i);
}
}
// -----------------------------------------------------------------------------
The Arduino setup() displays the version string and configures each servo pin as an output with a debug print.
void
setup (void)
{
Serial.begin(9600);
Serial.println (version);
for (int n = 0; n < N_SERVOS; n++) {
pinMode (servoPins [n], OUTPUT);
Serial.print ("setup: servo pin ");
Serial.print (servoPins [n]);
Serial.print (", time ");
Serial.println (ServoTimeMid);
}
}