Connecting servos to the Ardumoto shield on the Arduino

After my initial experiments with the Ardumoto motor shield, I've decided to extend it to drive the pan / tilt servos on my robot. The new servo library in release 17 of the Arduino software supports connecting servos to any of the digital out pins, however it needs to use Timer 1 (the only 16-bit timer on the Atmel ATmega168 chip) which is used for PWM of motor A on the Ardumoto.

This can be fixed by a fairly simple mod: re-wiring the Ardumoto to use pin 3 instead of pin 10. This way, both PWM channels use Timer 2, leaving Timer 3 available to use for analog output on pins 5 and 6.

Power for the servos is connected to the Vin pin on the Ardumoto to use the same external power supply, preventing noise on the Arduino's main 5V line. The board looks a bit messy, as I initially cut the wrong track and had to attempt a repair:

With the board mod complete, servos are connected, along with an external power supply (6V battery):

I've extended my Ardumoto test code to add servo support, and with the Arduino servo library it's very easy - servos are first "attached", then the position (in degrees or milliseconds) is set using a "write" command. The timing used for different servos varies - I've set things up for the Sanwa SRM-102 servo (700mS to 2300mS pulse) and Tower Pro SG-90 servo (600mS to 2100mS pulse).

// Test program for Sparkfun Ardumoto board with servos attached
// Copyright 2009 mechomaniac.com
 
// To use, connect the Arduino to a computer and send commans using a serial terminal.
// eg AR40#   motor A forwards with a speed of 40
//    S120#   servo 1 to position 20 degrees
 
 
#include <Servo.h>
 
#define PwmPinMotorA 3 // this has been moved from pin 11 on the original Ardumoto
#define PwmPinMotorB 11
#define DirectionPinMotorA 12
#define DirectionPinMotorB 13
#define SerialSpeed 9600
#define BufferLength 16
#define LineEnd '#'
#define PinServo1 7
#define PinServo2 8
#define PinServo3 9
#define DefaultServoPosition 90
 
Servo servo1;
Servo servo2;
Servo servo3;
char inputBuffer[BufferLength];
 
void setup()
{
  // motor and servo pins must be outputs
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
  pinMode(PinServo1, OUTPUT);
  pinMode(PinServo2, OUTPUT);
  pinMode(PinServo3, OUTPUT);    
  // attach servos and set servo-specific timing details
  servo1.attach(PinServo1, 600, 2100); // Tower Pro SG90 servo
  servo2.attach(PinServo2, 700, 2300); // Sanwa SRM-102 servo
  servo3.attach(PinServo3, 600, 2100); // Tower Pro SG90 servo
  // default servo positions
  servo1.write(DefaultServoPosition);
  servo2.write(DefaultServoPosition);
  servo3.write(DefaultServoPosition);  
 
  Serial.begin(SerialSpeed); 
}
 
// process a command string
void HandleCommand(char* input, int length)
{
  Serial.println(input);
  if (length < 2) { // not a valid command
    return;
  }
  int value = 0;
  // calculate number following command
  if (length > 2) {
    value = atoi(&input[2]);
  }
  int* command = (int*)input;
  // check commands
  // note that the two bytes are swapped, ie 'RA' means command AR
  switch(*command) {
    case 'FA':
      // motor A forwards
      analogWrite(PwmPinMotorA, value);
      digitalWrite(DirectionPinMotorA, HIGH);
      break;
    case 'RA':
      // motor A reverse
      analogWrite(PwmPinMotorA, value);
      digitalWrite(DirectionPinMotorA, LOW);
      break;
    case 'FB':
      // motor B forwards
      analogWrite(PwmPinMotorB, value);
      digitalWrite(DirectionPinMotorB, LOW);
      break;
    case 'RB':
      // motor B reverse
      analogWrite(PwmPinMotorB, value);
      digitalWrite(DirectionPinMotorB, HIGH);
      break;
    case '1S':
      // servo1      
      servo1.write(value);
      break;
    case '2S':
      // servo2  
      servo2.write(value);
      break;
    case '3S':
      // servo 3
      servo3.write(value);
      break;
    default:
      Serial.println("unknown command");
      break;
  }  
} 
 
void loop()
{ 
  // get a command string form the serial port
  int inputLength = 0;
  do {
    while (!Serial.available()); // wait for input
    inputBuffer[inputLength] = Serial.read(); // read it in
  } while (inputBuffer[inputLength] != LineEnd && ++inputLength < BufferLength);
  inputBuffer[inputLength] = 0; //  add null terminator
  HandleCommand(inputBuffer, inputLength);
}

User login