Arduino functions for Micro Serial Servo board

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
} 

Thanks Eustace! As Arduinos get more popular I’m sure lots of people are going to find these functions very helpful.

-Adam

I hope so! I put a description of my robot arm along with code samples on the Arduino forums too. Here’s a link to a pic - http://www.flickr.com/photos/24299497@N05/3128621913/

wow impressing work

is it possible for you to explain the connections (wiring) with arduino, the pololu chip and the power supply.
thank you

albatros