I'm including the code here that I wrote to demo the arm. It uses two button switches to cycle through the servos (and leds to show which is active) and a pot to adjust the servo over a range of motion limited by the readPot() function when it's called. I'm using Pololu protocol commands Set Speed (1) and Set Position, Absolute (4). In order to keep the ability to send the joint position values through the hardware serial port to the serial monitor, I used Software Serial to send commands to the Pololu board. And I hooked up both rx and tx though the connection only works one way (Arduino to serial servo board).
// Robot arm joint positioner
// Software Serial from Arduino example by Tom Igoe
// put() & set_speed() functions found on pololu forums & modified
// mike crist 2009-01-02
#include <SoftwareSerial.h>
#define rxPin 2
#define txPin 3
#define gripSwitch 4
#define upSwitch 5
#define downSwitch 6
#define gripLED 7
#define wristLED 8
#define elbowLED 9
#define shoulderLED 10
#define baseLED 11
#define potPin 0
#define baseServo 0
#define shoulderServo 1
#define elbowServo 2
#define wristServo 3
#define gripServo 4
int wristVal = 3050;
int elbowVal = 2400;
int shoulderVal = 1880;
int baseVal = 3000;
int gripVal = 1550;
int mode = 1;
// set up a new serial port
SoftwareSerial mySerial = SoftwareSerial(rxPin, txPin);
void setup()
{
pinMode(rxPin, INPUT);
digitalWrite(txPin, HIGH); // keeps pololu board from getting spurious signal
pinMode(txPin, OUTPUT);
pinMode(gripSwitch, INPUT);
pinMode(upSwitch, INPUT);
pinMode(downSwitch, INPUT);
pinMode(gripLED, OUTPUT);
pinMode(wristLED, OUTPUT);
pinMode(elbowLED, OUTPUT);
pinMode(shoulderLED, OUTPUT);
pinMode(baseLED, OUTPUT);
// set the data rate for the hardware serial port
Serial.begin(9600);
// set the data rate for the SoftwareSerial port
mySerial.begin(9600);
set_speed(0, 15);
set_speed(1, 10);
set_speed(2, 15);
set_speed(3, 55);
set_speed(4, 127);
put(baseServo, baseVal);
put(shoulderServo, shoulderVal);
put(elbowServo, elbowVal);
put(wristServo, wristVal);
put(gripServo, gripVal);
}
void loop()
{
// mode is set by up and down button switches
// mode 1 is base adjust thru mode 5 gripper adjust
if(digitalRead(upSwitch) == HIGH) // if the up switch is pressed
{
mode++;
if(mode == 6){mode = 1;}
}
if(digitalRead(downSwitch) == HIGH) // if the down switch is pressed
{
mode--;
if(mode == 0){mode = 5;}
}
switch (mode)
{
case 1: //base adjust mode
digitalWrite(baseLED, HIGH);
digitalWrite(shoulderLED, LOW);
digitalWrite(elbowLED, LOW);
digitalWrite(wristLED, LOW);
digitalWrite(gripLED, LOW);
baseVal = readPot(1145, 4800);
put(baseServo, baseVal);
break;
case 2: //shoulder adjust mode
digitalWrite(baseLED, LOW);
digitalWrite(shoulderLED, HIGH);
digitalWrite(elbowLED, LOW);
digitalWrite(wristLED, LOW);
digitalWrite(gripLED, LOW);
shoulderVal = readPot(1145, 3000);
put(shoulderServo, shoulderVal);
break;
case 3: //elbow adjust mode
digitalWrite(baseLED, LOW);
digitalWrite(shoulderLED, LOW);
digitalWrite(elbowLED, HIGH);
digitalWrite(wristLED, LOW);
digitalWrite(gripLED, LOW);
elbowVal = readPot(1570, 3340);
put(elbowServo, elbowVal);
break;
case 4: //wrist adjust mode
digitalWrite(baseLED, LOW);
digitalWrite(shoulderLED, LOW);
digitalWrite(elbowLED, LOW);
digitalWrite(wristLED, HIGH);
digitalWrite(gripLED, LOW);
wristVal = readPot(1275, 4760);
put(wristServo, wristVal);
break;
case 5: //gripper adjust mode
digitalWrite(baseLED, LOW);
digitalWrite(shoulderLED, LOW);
digitalWrite(elbowLED, LOW);
digitalWrite(wristLED, LOW);
digitalWrite(gripLED, HIGH);
gripVal = readPot(1550, 4750);
put(gripServo, gripVal);
}
printOut();
//delay(100);
}
void put(int servo, int angle)
{
//the put function uses pololu mode command 4 to set absolute position
//servo is the servo number (typically 0-7)
//angle is the absolute position from 500 to 5500
unsigned int temp;
byte pos_hi,pos_low;
temp = angle & 0x1f80; //get bits 8 thru 13 of position
pos_hi = temp >> 7; //shift bits 8 thru 13 by 7
pos_low = angle & 0x7f; //get lower 7 bits of position
mySerial.print(0x80,BYTE); //start byte
mySerial.print(0x01,BYTE); //device id
mySerial.print(0x04,BYTE); //command number
mySerial.print(servo,BYTE); //servo number
mySerial.print(pos_hi, BYTE); //bits 8 thru 13
mySerial.print(pos_low, BYTE); //bottom 7 bits
}
void set_speed(byte servo, byte speedVal)
{
//servo is the servo number (typically 0-7)
//speedVal is servo speed (1=slowest, 127=fastest, 0=full)
//set speedVal to zero to turn off speed limiting
speedVal = speedVal & 0x7f; //take only lower 7 bits of the speed
//Send a Pololu Protocol command
mySerial.print(0x80,BYTE); //start byte
mySerial.print(0x01,BYTE); //device id
mySerial.print(0x01,BYTE); //command number
mySerial.print(servo,BYTE); //servo number
mySerial.print(speedVal,BYTE); //speed
}
void grip_open()
{
put(4, 4260);
}
void grip_close()
{
put(4, 1550);
}
int readPot(int lowNum, int highNum)
{
int potVal = analogRead(potPin);
potVal = map(potVal, 0, 1023, lowNum, highNum);
return potVal;
}
void printOut()
{
Serial.print("gripper = ");
Serial.print(gripVal);
Serial.print(" wrist = ");
Serial.print(wristVal);
Serial.print(" elbow = ");
Serial.print(elbowVal);
Serial.print(" shoulder = ");
Serial.print(shoulderVal);
Serial.print(" base = ");
Serial.println(baseVal);
}
