Controlling Servo Speed

Ok, so I can’t figure this out. I don’t see anything I am doing wrong to change the servo speed but it doesn’t work. Here is what I have got.

int stepToSlow(ssc *s, int controller, int servo, int pos) {
	if(!s->connected)
		return 0;

	if(pos < 0)
		sendCommand5(s,(char)controller,0,(char)servo,(char)0x0F); //turn servo off
	else{
		sendCommand5(s,(char)controller,0,(char)servo,0x4F); //turn servo on
		usleep(100); //wait for signal to propogate
		sendCommand5(s,(char)controller,1,(char)servo,(char)1); //set speed
		usleep(100); //wait for signal to propogate
		sendCommand6(s,(char)controller,4,(char)servo,(char)(pos/128),(char)(pos%128)); //give position
	}


	return 1;
 }

Yes the position and everything is getting passed in fine because the servo moves to the proper location, it just doesn’t does it at the default speed. sendCommand5() is the exact same as sendCommand(). I just didn’t see that there was a sendCommand() function that did what I needed it to do.

Hello,

You cannot control the speed of a servo’s very first move. Does the speed control not work for subsequent moves, too?

- Jan

Nope, actually the servo gets moved to 0 degrees when the program starts without the speed being set. Would that effect it any?

Ok here is all the code I am using, its pretty short so hopefully it will be easy to debug. Not sure if this matters but I’m running this code in Ubuntu 8.04

Note I did take out the servo moving to 0 degrees at start of program that I mentioned in my previous post.

//includes here

#define MAX_VAL 5000
#define MIN_VAL 1200
#define DEGREE 180
#define COUNT_PER_DEGREE (MAX_VAL-MIN_VAL)/DEGREE


void sendCommand5(ssc *s, char controller, char cmd, char servo, char data1) {
	char buf[5];
	servo += controller*16;
	snprintf(buf,5,"%c%c%c%c%c",0x80,0x01,cmd,servo,data1);
	write(s->fd,buf,5);
}

int main(){
	ssc s;
	char portName[100];
	int controller = 0, servo;
	double pos0, angle;
	
	int counter = 0;
	while(1){
		sprintf(portName, "/dev/ttyUSB%d", counter);
		if(connectSsc(&s, portName) == 1){
			printf("Connected to : %s\n", portName);
			break;
		}
		counter++;
	}

	enable(&s, controller, 0, 1);
	
	while(1){
		//get servo/angle input from user
		printf("Enter Servo: ");
		scanf("%d", &servo);
		printf("Enter angle: ");
		scanf("%lf", &pos0);
		
		if(pos0 < 0)
			sendCommand5(&s,(char)controller,0,(char)servo,(char)0x0F); //turn servo off
		else{
			//set angle to nearest 1.8 degrees
			angle = pos0/1.8;
			angle = (int)angle;
			angle *= 1.8;

			//convert to a number from 1200-5000
			angle = angle*COUNT_PER_DEGREE + MIN_VAL;
			sendCommand5(&s,(char)controller,0x01,(char)servo,0x01); //set speed
			usleep(1000);
			stepTo(&s, controller, servo, (int)angle);
		}
		
	}
	endSession(&s);
}

Anybody have any thoughts?

Could sombody perhaps post some code where they implemented the speed control and it worked?

-Brian Peasley

Sorry you’re having so much trouble. I don’t have my servo controller here with me, so I don’t want to post an untested C function, but here’s a simple line from a Matlab program:

fwrite(ser,[128,1,1,3,4]);%set wrist (servo 3) to speed 4

And here’s a more generic function from some Arduino code:

void servoSetSpeed(unsigned char servo, unsigned char speedcmd){
   //servo is the servo number (typically 0-7)
   //speed is servo speed (1=slowest, 127=fastest)
   //set speed to zero to turn off speed limiting
   
   speedcmd=speedcmd&0x7f;//take only lower 7 bits of the speed
   
   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE);//start byte
   Serial.print(0x01,BYTE);//device id
   Serial.print(0x01,BYTE);//command number
   Serial.print(servo,BYTE);//servo number
   Serial.print(speedcmd,BYTE);//data1
}

Does your computer have hardware serial ports? You could connect them with a crossover cable, point your code at one and look at the bytes its outputting to the other with a terminal program (or just connect pin3 of the transmitter to pin2 of the receiver if you don’t have a crossover cable handy).

-Adam

Yea It looks like I’m going to have to end up hooking up a terminal to my output so I can see what is getting sent. But the posistion works fine, which leads me to believe that the servo controller is getting the write command, but I’m just sending it wrong.

-Brian Peasley

Ok, so I tried it with the Windows Utility Transmitter and it works, but I can’t find out what is going wrong in my code.

-Brian Peasley

Are you running this code on Windows? From your other thread I think I had it in the back of my mind that you were doing this in Linux. In that case I would recommend HDD Free Serial Port Mointor to help you figure out what’s going on (i.e. exactly what bytes your program is sending out, how often, etc…).

If you start it up first and point it at the serial port you’re using (real or virtual), then run your servo controller program, it will display all of the bytes being sent to your serial port, kind of like a digital wire-tap. I use this at least once a week to diagnose serial issues and its great to be able to snoop in on two-way communication with actual hardware.

-Adam

P.S. More details on the serial port monitor and some other useful serial bytes on this thread.

Ok I’ll look into that, and yes I am running my code on Ubuntu 8.04

-Brian Peasley

Well this is great, now the damn thing isn’t even moving. Haven’t changed a thing. Sending it pretty basic stuff that once worked, and now just sits there. However it works when I send a command to it through the windows serial utility. Tried the code on two linux machines and doesn’t do anything.

int main(){
	ssc s;
	char portName[100];
	int controller = 1, servo , speed = 0;
	double pos0, angle;
	
	int counter = 0;
	while(1){
		sprintf(portName, "/dev/ttyUSB%d", counter);
		if(connectSsc(&s, portName) == 1){
			printf("Connected to : %s\n", portName);
			break;
		}
		counter++;
	}

	sendCommand6(&s, (char)controller, (char)4, (char)0, (char)0x1B, (char)0x08); //go 120ish degrees
	usleep(2000000);
	sendCommand6(&s, (char)controller, (char)4, (char)0, (char)9, (char)30); //go to 0ish degrees
	usleep(2000000);
	sendCommand5(&s,(char)controller,0,(char)0,(char)0x0F); //turn servo off
	usleep(1000000);
	endSession(&s);
}

int connectSsc(ssc *s, char *port) {

	struct termios options;

	int count,ret;



	s->connected=0; /* reset in case we fail to connect */



	s->fd = open(port, O_RDWR | O_NOCTTY);

	if (s->fd == -1) return 0;

	else fcntl(s->fd, F_SETFL, 0);



	tcgetattr(s->fd, &options);



	/* go to 9600 baud */

	cfsetispeed(&options, B9600);

	cfsetospeed(&options, B9600);



	options.c_cflag |= (CLOCAL | CREAD); /* enable */



	options.c_cflag &= ~PARENB; /* 8N1 */

	options.c_cflag &= ~CSTOPB;

	options.c_cflag &= ~CSIZE;

	options.c_cflag |= CS8;



	/* set all of the options */

	tcsetattr(s->fd, TCSANOW, &options);



	s->connected=1;

	return 1;

}

void sendCommand6(ssc *s, char controller, char cmd, char servo, char data1, char data2) {
	char buf[6];
	servo += controller*16;
	printf("Data1: %x, Data2: %x\n", data1, data2);
	snprintf(buf,6,"%c%c%c%c%c%c",0x80,0x01,cmd,servo,data1,data2);
	write(s->fd,buf,6);
}

The program connects to the board, the green lights that have normally have come on still coming on. And the servo still has power, like I said it was working with Windows.

-Brian Peasley

Ok I got everything working.

-Brian Peasley

Fantastic! What turned out to be the problem?

-Adam

snprintf() was doing what it was supposed to do for some reason.

I said snprintf(buf,5,"%c%c%c%c%c",0x80,id,cmd,servo,data); (all those parameters were right, printed out what they were)
then I printed out each index in buf, and no matter what I did data was always 0x00. So I just did

void sendCommand5(ssc *s, char controller, char cmd, char servo, char data) {
	char buf[5];
	//snprintf(buf,5,"%c%c%c%c%c",0x80,0x01,cmd,servo,data);
	buf[0] = 0x80; buf[1] = 0x01; buf[2] = cmd; buf[3] = servo; buf[4] = data;
	write(s->fd,buf,5);
}

and it works.