I figured it was worth reposting this somewhere it would get seen. If anyone is interested in a simple, editable, and compiler independent C program that will run any of the Pololu Servo Controllers from a Windows computer serial port (or USB-to-Serial adapter) I have mangled together my old quick testing programs into something marginally useful. You can download the source code here, or copy the text of it below.
To start, all you have to do is make sure the port number in the line:
comPort=openPort(1);//***PUT YOUR COM PORT NUMBER HERE!***
matches the com port your servo controller is connected to. For example, for COM3, you would change that line to read:
comPort=openPort(3);
The program will make the servos move a bit, then end in a menu that lets you adjust all the servos with the keyboard, printing out the servo numbers and positions. If you note the servo positions you want to reproduce, you can add them into the beginning of the program.
To add automated motion at the beginning of the program, you can edit the part of the code that reads:
//TO START, make simple changes to the example control commands below
neutral();//send all servos to their neutral positions
waitMS(1000);//wait 1 second
put(0,3500);//send servo 0 to position 3500
With combinations of these three functions, you should be able to do some pretty neat stuff, like a gait for a walking robot. The program uses the Pololu communication mode (no protocol selection jumper!) and the absolute position command. If anyone is interested I could be persuaded to add other functions, like one to use the Pololu servo speed control setting.
If you’re looking for a good (free) C compiler, I use DevC++, and if you’re curious about the serial protocol stuff, I got it all from this document.
Please post any reccomendations/experiences/bugs!
-Adam
/*
Pololu Serial Servo Controller Test Program
V 1.0
Adam Borrell
7/05/07
*/
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <conio.h>
#include <time.h>
//IMPORTANT FUNCTION PROTOTYPES with descriptions
HANDLE openPort(int);//Serial protocol stuff, don't worry about it
HANDLE comPort;//Serial protocol stuff, don't worry about it
void put(int servo, int angle);//Moves servo (0-7) to angle (500-5500)
void servoOff();//Stops signal to all servos, analog servos will go slack, digital servos wont!
void neutral();//Sends all servos to halfway between max and min
void waitMS(int ms);//Milisecond delay function
static int min=500, max=5500, step=50;//Decrease steop size for finer control
int angles[8]={3000,3000,3000,3000,3000,3000,3000,3000};
int done=0;
int main(){
char input[32];
int i;
printf("Pololu Serial Servo Controller Test Program\n\n");
comPort=openPort(1);//***PUT YOUR COM PORT NUMBER HERE!***
if(done){
printf("Program Terminated!\n");
system("PAUSE");
return;
}
//TO START, make simple changes to the example control commands below
neutral();//send all servos to their neutral positions
waitMS(1000);//wait 1 second
put(0,3500);//send servo 0 to position 3500
//LEAVE EVERYTHING BEYOND HERE ALONE to start, you can muck with it later
printf("Commmands:\n");
printf("A - Increase servo 0 position\n");
printf("Z - Decrease servo 0 position\n");
printf("S - Increase servo 1 position\n");
printf("X - Decrease servo 1 position\n");
printf(".........\n");
printf("K - Increase servo 1 position\n");
printf("< - Decrease servo 1 position\n");
printf("Spacebar - Emergency Servo Stop (WARNING: no effect on digital servos!)\n");
printf("Backspace - quit\n\n");
while (!done) {//***Simple keyboard interface
*input = getch();
switch(*input){
case ' '://spacebar
servoOff();
printf("Emergency Servo Stop\n");
break;
case 8://backspace
done = 1;
break;
case'a':
case'A':
i=0;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'z':
case'Z':
i=0;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case's':
case'S':
i=1;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'x':
case'X':
i=1;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case'd':
case'D':
i=2;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'c':
case'C':
i=2;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case'f':
case'F':
i=3;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'v':
case'V':
i=3;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case'g':
case'G':
i=4;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'b':
case'B':
i=4;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case'h':
case'H':
i=5;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'n':
case'N':
i=5;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case'j':
case'J':
i=6;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case'm':
case'M':
i=6;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
case'k':
case'K':
i=7;
if(angles[i]<=(max-step)) angles[i]+=step;
put(i,angles[i]);
break;
case',':
case'<':
i=7;
if(angles[i]>=(min+step)) angles[i]-=step;
put(i,angles[i]);
break;
default:
break;
}
}
servoOff();
printf("Program Terminated!\n");
system("PAUSE");
return;
}
void neutral(){
int i;
for(i=0;i<9;i++){
put(i,(max+min)/2);
}
printf("\n");
}
void put(int servo, int angle){
unsigned char buff[6];
DWORD len;
unsigned short int temp;
unsigned char pos_hi,pos_low;
temp=angle&0x1f80;
pos_hi=temp>>7;
pos_low=angle & 0x7f;
buff[0]=0x80;//start byte
buff[1]=0x01;//device id
buff[2]=0x04;//command number
buff[3]=servo;//servo number
buff[4]=pos_hi;//data1
buff[5]=pos_low;//data2
WriteFile(comPort, &buff, 6, &len, 0);
printf("Servo %d Set to %d\n", servo, angle);
return;
}
void servoOff(){
unsigned char buff[6];
DWORD len;
int servo;
buff[0]=0x80;//start byte
buff[1]=0x01;//device id
buff[2]=0x00;//command number
buff[4]=0x0f;//data1
for(servo=0;servo<8;servo++) {
buff[3]=servo;//servo number
WriteFile(comPort, &buff, 5, &len, 0);
}
return;
}
void waitMS(int ms){
clock_t endwait;
endwait=clock()+ms*CLOCKS_PER_SEC/1000;
while(clock()<endwait);
}
HANDLE openPort(int portnum){
char port[]="com", pnum[]="Error";
itoa(portnum,pnum,10);
strcat(port,pnum);
HANDLE serial=CreateFile(port,GENERIC_READ|GENERIC_WRITE,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0);
if(serial==INVALID_HANDLE_VALUE){
if(GetLastError()==ERROR_FILE_NOT_FOUND){
printf("Error, %s Not Found\n", port);
done=1;
return;
}
printf("Com Error\n");
done=1;
return;
}
DCB dcbSerialParams={0};
dcbSerialParams.DCBlength=sizeof(dcbSerialParams);
if(!GetCommState(serial, &dcbSerialParams)){
printf("Com State Error\n");
done=1;
return;
}
dcbSerialParams.BaudRate=CBR_19200;//CBR_baudrate
dcbSerialParams.ByteSize=8;
dcbSerialParams.Parity=NOPARITY;//NOPARITY, ODDPARITY, EVENPARITY
dcbSerialParams.StopBits=ONESTOPBIT;//ONESTOPBIT, ONE5STOPBITS, TWOSTOPBITS
if(!SetCommState(serial, &dcbSerialParams)){
printf("Serial Protocol Error\n");
done=1;
return;
}
COMMTIMEOUTS timeouts={0};
timeouts.ReadIntervalTimeout=50;
timeouts.ReadTotalTimeoutConstant=50;
timeouts.ReadTotalTimeoutMultiplier=10;
timeouts.WriteTotalTimeoutConstant=50;
timeouts.WriteTotalTimeoutMultiplier=10;
if(!SetCommTimeouts(serial,&timeouts)){
printf("Timeout Setting Error\n");
done=1;
return;
}
return serial;
}