While looking for example code to help me get my Arduino/Pololu Micro Serial Controller robot arm working, I found the put() and set_speed() commands here on the forums. For other Arduino users, I thought I would post code for all six of the commands (the Pololu protocol commands, that is). I renamed put() to position_absolute() just to be consistent (and I got rid of the array dump to serial code for the same reason). I’m using the software serial library to connect the servo board to pins other than the hardware serial pins (this keeps the hardware serial port free for debugging uses).
// Pololu micro serial servo controller modes
// Software Serial from Arduino example by Tom Igoe
// put() (now position_absolute) & set_speed() functions found on pololu forums & modified
// mike crist 2009-01-03
#include <SoftwareSerial.h>
#define rxPin 2
#define txPin 3
// set up a new serial port
SoftwareSerial softSerial = SoftwareSerial(rxPin, txPin);
void setup()
{
pinMode(rxPin, INPUT);
digitalWrite(txPin, HIGH); // keeps pololu board from getting spurious signal
pinMode(txPin, OUTPUT);
// set the data rate for the hardware serial port
Serial.begin(9600);
// set the data rate for the SoftwareSerial port
softSerial.begin(9600);
// set initial servo speeds
set_speed(0, 10);
set_speed(1, 10);
set_speed(2, 10);
set_speed(3, 10);
//set and move servos to neutral positions using command 5
set_neutral(0, 3000);
set_neutral(1, 3000);
set_neutral(2, 3000);
set_neutral(3, 3000);
//initialize servos using command 0
set_servo_parameters(0, 15);
set_servo_parameters(1, 15);
set_servo_parameters(2, 15);
set_servo_parameters(3, 15);
set_servo_parameters(4, 15);
}
void loop()
{
}
void set_servo_parameters(byte servo, byte rangeVal)
{
//this function uses pololu mode command 0 to set servo parameters
//servo is the servo number (typically 0-7)
//rangeVal of 15 gives 180 deg in 8-bit, 90 deg in 7 bit
byte temp;
byte parameters;
temp = 1 << 6; //set first two bits of parameters (on = 1, forward = 0)
temp = temp + (rangeVal & 0x1f); //put first five bits of rangeVal into temp
parameters = temp & 0x7f; //take only bottom 7 bits
//Send a Pololu Protocol command
softSerial.print(0x80,BYTE); //start byte
softSerial.print(0x01,BYTE); //device id
softSerial.print(0x00,BYTE); //command number
softSerial.print(servo,BYTE); //servo number
softSerial.print(parameters,BYTE); //parameters
}
void set_speed(byte servo, byte speedVal)
{
//this function uses pololu mode command 1 to set speed
//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
softSerial.print(0x80,BYTE); //start byte
softSerial.print(0x01,BYTE); //device id
softSerial.print(0x01,BYTE); //command number
softSerial.print(servo,BYTE); //servo number
softSerial.print(speedVal,BYTE); //speed
}
void position_7bit(byte servo, byte posValue)
{
//this function uses pololu mode command 2 to set position
//servo is the servo number (typically 0-7)
//posValue * range (set with command 0) adjusted by neutral (set with command 5)
//determines servo position
byte pos = posValue & 0x7f; //get lower 7 bits of position
//Send a Pololu Protocol command
softSerial.print(0x80,BYTE); //start byte
softSerial.print(0x01,BYTE); //device id
softSerial.print(0x02,BYTE); //command number
softSerial.print(servo,BYTE); //servo number
softSerial.print(pos, BYTE); //position
}
void position_8bit(byte servo, byte posValue)
{
//this function uses pololu mode command 3 to set position
//servo is the servo number (typically 0-7)
//posValue * range (set with command 0) adjusted by neutral (set with command 5)
//determines servo position
byte temp;
byte pos_hi,pos_low;
temp = posValue & 0x80; //get bit 8 of position
pos_hi = temp >> 7; //shift bit 8 by 7
pos_low = posValue & 0x7f; //get lower 7 bits of position
//Send a Pololu Protocol command
softSerial.print(0x80,BYTE); //start byte
softSerial.print(0x01,BYTE); //device id
softSerial.print(0x03,BYTE); //command number
softSerial.print(servo,BYTE); //servo number
softSerial.print(pos_hi, BYTE); //bits 8 thru 13
softSerial.print(pos_low, BYTE); //bottom 7 bits
}
void position_absolute(byte servo, int angle)
{
//this 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
//Send a Pololu Protocol command
softSerial.print(0x80, BYTE); //start byte
softSerial.print(0x01, BYTE); //device id
softSerial.print(0x04, BYTE); //command number
softSerial.print(servo, BYTE); //servo number
softSerial.print(pos_hi, BYTE); //bits 8 thru 13
softSerial.print(pos_low, BYTE); //bottom 7 bits
}
void set_neutral(byte servo, int angle)
{
//this function uses pololu mode command 5 to set neutral 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
//Send a Pololu Protocol command
softSerial.print(0x80, BYTE); //start byte
softSerial.print(0x01, BYTE); //device id
softSerial.print(0x05, BYTE); //command number
softSerial.print(servo, BYTE); //servo number
softSerial.print(pos_hi, BYTE); //bits 8 thru 13
softSerial.print(pos_low, BYTE); //bottom 7 bits
}