Okay, I tried to clean this up and comment it a little bit. It’s never good to do this, but I haven’t actually tested this on a servo controller in its current state (my servo controller is at work, of course!).
I’ll paste the complete source code below, or you can download a tab-formatted version of the source code here.
The only parts you’ll want to mess with to start are inside the “main” function. You’ll need to change the line:
comPort=openPort(1);//PUT YOUR COM PORT NUMBER HERE!
to use the number of your com port, and you’re ready to try it out. It has a simple keyboard interface to start, and it will print out what keys do what.
It has a few functions like put(servo, angle), which sets servo number servo to angle angle, and neutral() which does what it sounds like (comments are pretty explicit too). All of these functions use the Pololu absolute position command, but you might want to try using other things like the servo speed control commands later.
There is also a millisecond delay function waitMS(), which you may find useful in generating your walking code (for example, waitMS(1000) will cause a 1 second delay before the next command).
If you have trouble getting this to compile in MS Visual C, you might try the free C and C++ compiler I use, DevC++
Welcome to console programming in C!
-Adam
[code]/*
Pololu Serial Servo Controller Test Program
Adam Borrell
7/02/07
*/
#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <conio.h>
#include <time.h>
//IMPORTANT FUNCTION PROTOTYPES
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;
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;
}
waitMS(1000);//***Useless example of the wait function***
neutral();
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;
}
[/code]
<-Power 6V
