usb16: code to control servo speed doesn't work

Hello… again

After a long hard search in the forums, I still don’t get why my code tryign to set servo speed doesn’t work. I’ve written something very similar to Adam’s code (at least that I think of), but whatever the speed (1->127, or even 0) I ask in main(), I dont’ see any difference .

Here’s the setSpeed function I added, and the sendCommand function is the one in the linux-Tutorial:

int setSpeed(ssc *s, int controller, int servo, int speed) {
  char speed_char;

  if(!s->connected) { return 0; }

  speed_char=(char)(speed)&0x7f;
  sendCommand(s, (char)controller, 1, (char)servo, speed_char);
  return 1;
}

Any insight on this issue ?

__
Den

Also, does the setting of the speed affect the behavior of the servo in Absolute mode ?

The speed setting will work for all position commands in Pololu mode. From the variable names I can’t tell what bytes your function is sending out, but it doesn’t look it’s sending out enough of them. The speed command should have five bytes in this order:

0x80 - start byte always 0x80
0x01 - device id 0x01 for servo controllers
0x01 - command number, 0x01 to set speed
Servo Number - servo number to apply command to
Speed Command - New Speed (1=slowest, 127=fastest, 0 turns off speed limiting)

-Adam

Hello,

Does your sendCommand() function add the 0x80 first byte? It’s generally good to program in terms of variables and constants that you can easily change later, but if you want a simple example to test and to show others, it would be more helpful if you showed what you’re sending more explicitly. In addition to sendCommand(), we also don’t know what controller, servo, and speed are, and a bad value in any one of them would prevent the unit from having the behavior you expect.

The speed does affect absolute mode. Note that the first position command you send can cause the servo to move quickly since the servo controller cannot initially know where the servo is.

- Jan

OK sorry for not posting the entire code, here it is.
In fact, the sendCommand() function takes care of the first 2 bytes.

/* functions for communicating with the pololu ssc over a serial port */

#include <stdio.h>   /* Standard input/output definitions */
#include <stdlib.h>
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <sys/select.h>
#include "ssc.h"

int main(int argc, char **argv){
   ssc s;

   char portName[100];
   char pos_string[100];
   int controller =0;
   int servo = 0;
   int pos =1;
   sprintf(portName, "/dev/ttyUSB0");

   connectSsc(&s,portName);
   enable(&s, controller, servo, 1);//controller, servo, enabled
   while(pos>0) {
   setSpeed(&s, controller, servo, 1);

	scanf("%s",&pos_string);
	pos=atoi(pos_string);
	if(pos >0) stepTo(&s,controller,servo,atoi(pos_string));
   }
   endSession(&s);
}

int connectSsc(ssc *s, char *port) {
  struct termios options;
  int count,ret;

  s->connected=0; /* reset in case we fail to connect */
  printf("ok\n");

  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;
}

/* write a message to the SSC */
void sendCommand(ssc *s, char id, char cmd, char servo, char data) {
  char buf[5];
  snprintf(buf,5,"%c%c%c%c%c",0x80,0x01,cmd,servo,data);
  write(s->fd,buf,5);
}

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

int enable(ssc *s, int controller, int servo, int enabled) {
  if(!s->connected) { return 0; }

  if(enabled) {
    sendCommand(s,(char)controller,0,(char)servo,(char)0xffff);
    printf("enabled %i\n",servo); fflush(stdout);
  } else {
    sendCommand(s,(char)controller,0,(char)servo,0x00);
    printf("disabled %i\n",servo); fflush(stdout);
  }

}

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

  sendCommand6(s,(char)controller,0x04,(char)servo,(char)(pos/128),(char)(pos%128));

  return 1;
}

int setSpeed(ssc *s, int controller, int servo, int speed) {
  char speed_char;

  if(!s->connected) { return 0; }
  speed_char=(char)(speed)&0x7f;
  sendCommand(s, (char)controller, 0x01, (char)servo, speed_char);
  return 1;
}

void endSession(ssc *s) {
  if(!s->connected) return;
  close(s->fd);
}



const char *getId(ssc *s) {
  return s->idstring;
}

I think the sequence of characters sent in setSpeed() is OK, but I might be wrong.

Yes, I noticed that. But anyway, the loop in main() allows me to try many values after the initial connection. With poor results regarding the speed, for the moment :wink:

At a glance nothing looks wrong with your code, and you’ve got the servos to move at normal speed, so at least that much is working right.

I’ve found that many servos can’t keep up with the higher speeds anyway, so you won’t see any difference there. If you try setting the speed to 1 (slowest) then try moving the servos, do you see a change?

-Adam

No, no difference whatever the speed parameter.
I’d like to obtain slow, fluid motions, so my interest here lies in the smallest speeds…

__
Den

Could anyone try out this simple code on their setup, without too many efforts?
It would be lovely :wink:

I’m trying to find out whether the problem comes from my code or from my setup.

(or both of them?)

__
Den

I don’t have a good setup for testing your code, but I think you could vastly simplify it to make it easier for others to look it over. If you reduce it to a single main function, and don’t use extra variable and constant names, it should be a dozen or two lines that’s easy for us to look over. Since you do have some servo control, it’s extremely unlikely that there is a defect that causes just the speed part to fail, so I think it is going to be a result of a problem in your code. It might help you, too, to have a super-simple case that you can always go back to as a sanity check.

- Jan

OK Jan, I followed your advice, and here’s the new code, I hope someone will fond what’s wrong with it.
It only consists in:
1 - main(),
2 - connectSsc()
3 - endSession(),
functions 2 and 3 being copied out from the Linux Tutorial.

The main() function procedes as follows:

  • Connect to the usb16sc
  • Set speed to 1
  • Set position (absolute) to pos1
  • Set position (absolute) to pos2
  • Wait 2 seconds
  • Set speed to 127
  • Set position (absolute) to pos1
  • Set position (absolute) to pos2

I found that the commands were much more “understood” if I add at least 100us between two orders, that’s why I added usleep() calls between each sending.

Anyway, and again with this new code, I see no difference regarding the speed of execution of the 2 moves pos1->pos2, executed respectively at speed 1 and 127. I tried this code on all four servos, with different sets of speed (1,2, 10, 16, and 0), of positions, of baud rates (9600-19200),… I’m guessing that I don’t send an understandable “speed” command?

Could it be due the options chosen at the connection through the serial port?

#include <stdio.h>   /* Standard input/output definitions */
#include <stdlib.h>
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <sys/select.h>
#include "ssc.h"

#define PI  3.14159265

int main(int argc, char **argv){
   ssc s;
   char portName[100];

   char buffer5[5];
   char buffer6[6];
   int pos1=1400;
   int pos2=4700;
   int myDelay=1000000;

   sprintf(portName, "/dev/ttyUSB0");

   //first, connect the SSC (see below the connectSsc() function
   connectSsc(&s,portName);

   //set params for servo #0 (not really used here)
   snprintf(buffer5,5,"%c%c%c%c%c",0x80,0x01,0x00,0x00,0x40);
   write((&s)->fd,buffer5,5);
   printf("Params set.\n");
   usleep(myDelay);

   //set servo #0 speed to 1 for instance
   snprintf(buffer5,5,"%c%c%c%c%c",0x80,0x01,0x01,0x00,0x01);
   write((&s)->fd,buffer5,5);
   printf("Speed set to 1.\n");
   usleep(myDelay);

   //step to position <pos1> (absolute mode, 6 bytes)
   snprintf(buffer6,6,"%c%c%c%c%c%c",0x80,0x01,0x04,0x00,(char)(pos1/128),(char)(pos1%128));
   write((&s)->fd,buffer6,6);
   printf("Order 1 sent.\n");
   usleep(myDelay);

   //then step to position <pos2> (absolute mode, 6 bytes)
   snprintf(buffer6,6,"%c%c%c%c%c%c",0x80,0x01,0x04,0x00,(char)(pos2/128),(char)(pos2%128));
   write((&s)->fd,buffer6,6);
   printf("Order 2 sent.\n");

   //wait 2 sec before the rest
   sleep(2);

   //set servo #0 speed to 127
   snprintf(buffer5,5,"%c%c%c%c%c",0x80,0x01,0x01,0x00,0x7f);
   write((&s)->fd,buffer5,5);
   printf("Speed set to 127.\n");
   usleep(myDelay);

   //step to position <pos1> (absolute mode, 6 bytes)
   snprintf(buffer6,6,"%c%c%c%c%c%c",0x80,0x01,0x04,0x00,(char)(pos1/128),(char)(pos1%128));
   write((&s)->fd,buffer6,6);
   printf("Order 3 sent.\n");
   usleep(myDelay);

   //then step to position <pos2> (absolute mode, 6 bytes)
   snprintf(buffer6,6,"%c%c%c%c%c%c",0x80,0x01,0x04,0x00,(char)(pos2/128),(char)(pos2%128));
   write((&s)->fd,buffer6,6);
   printf("Order 4 sent.\n");
   usleep(myDelay);

   endSession(&s);
}

int connectSsc(ssc *s, char *port) {
  struct termios options;
  int count,ret;

  s->connected=0; /* reset in case we fail to connect */
  printf("ok\n");

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

  if (s->fd == -1) {
  printf("Could not open connexion to Pololu USB16sc.\n");
  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;
  printf("Connection to Pololu USB16sc open.\n");
  return 1;
}

void endSession(ssc *s) {
  if(!s->connected) return;
  close(s->fd);
  printf("Connection to Pololu USB16sc closed.\n");
}

Thanks for the help.

- Den

Have you tried our serial transmitter utility yet? (I think I mentioned that in the other thread.)

- Jan

Well, is it not only for Windows ? Or is there a source I didn’t see?
I’m working on Debian.

Yeah, it’s just for Windows, but can you get access to a Windows machine to try it out?

- Jan