C code for Micro Maestro 6-Channel USB Servo Controller

Hi,

Firstly i would like to say that i am new to this forum and as such apologize if i make any mistakes in developing this topic.

I am a student at University and have chosen to develop a driver for the Pololu 6 servo USB controller. I have developed the following code which sets up a serial connection. Once connected i have set code in the main function to call the servo and set it at a specific position. This is done by sending the commands as explained in the pololu documantation.


 /* Better port reading program
 v1.0
 23-6-2010
 
 Uses termio functions to initialise the port to 9600 baud, at
 8 data bits, no parity, no hardware flow control,
 and features character buffering.
 */

#include <stdio.h>   /* Standard input/output definitions */
#include <stdlib.h>
#include <time.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 */

void put(int servo, int angle);	// Moves servo (0-5) to angle (500-5500)
void waitMS (int ms); // This allows for a set system instruction pause. Note: 1000ms = 1 sec.
void neutral(); // This sets up all the servos neutral positions before use.

typedef unsigned int DWORD;
int mainfd = 0;	/* File descriptor for the port */
char chout;
int angles[6] = {3000, 3000, 3000, 3000, 3000, 3000}; // Holds all the values for every servo at setup.
static int min = 500, max = 5500, step = 50; // Decrease step size for finer control.

/*
 * 'open_port()' - Open the serial port.
 * Returns the file descriptor on success or -1 on error.
 */
int open_port (char portName[]) {
	/*
	 * The open function is built as follows...
	 * 	1. The name of the serial port as a C-string (i.e. char *) eg. /dev/ttyso
	 *	2. Configuration options...
	 *		O_RDWR - we need read and write access
	 *		O_CTTY - prevent other input like keyboard from affecting what we read
	 *		O_NDELAY - we don't care if the other side is connected (some devices don't explicitly connect)
	 */
	int fd;	/* File descriptor for the port */
	fd = open(portName, O_RDWR | O_NOCTTY | O_NDELAY);
	
	if (fd == -1) {  /* Could not open the port */
		fprintf(stderr, "\nopen port: Unable to open %s - %s\n\n", portName, strerror(errno));
		exit(EXIT_FAILURE); // EXIT
	}
	else {
		printf("\nopen port: port %s has been opened correctly.\n\n", portName);
		printf("fd = %i\n\n", fd);
	}
	
	return (fd);
}

/*
 * 'config_port()' - Configure the serial port.
 */
void config_port(int mainfd) {
	/*
	 * Configuring the port is harder in Linux over that of other OS's.
	 * There is more bit masking involved in order to change a single option.
	 * While there are convienient functions that can be used to set the speed of the port, other options,like 
	 * parity and number of stop bits, are set using the c-cflag member of the termios struct, and require bitwise
	 * operations to set the various settings.
	 * Linux is also capable of setting the read time-out values. This is set using the c-cc member of the termios 
	 * struct which is actually an array indexed by defined values.
	 */
	// Create the struct
	struct termios options;
	
	// Get the current settings of the serial port.
	tcgetattr (mainfd, &options);

	// Set the read and write speed to 19200 BAUD.
	// All speeds can be prefixed with B as a settings.
	cfsetispeed (&options, B9600);
	cfsetospeed (&options, B9600);

	// Now to set the other settings. Here we use the no parity example. Both will assumme 8-bit words.

	// PARENB is enabled parity bit. This disables the parity bit.
	options.c_cflag &= ~PARENB;
	
	// CSTOPB means 2 stop bits, otherwise (in this case) only one stop bit.
	options.c_cflag &= ~CSTOPB;

	// CSIZE is a mask for all the data size bits, so anding with the negation clears out the current data size setting.
	options.c_cflag &= ~CSIZE;

	// CS8 means 8-bits per work
	options.c_cflag |= CS8;
}

/*
 * 'close_port(int mainfd, char portName)' - Close the serial port.
 */
void close_port(int mainfd, char portName[]) {
	/* Close the serial port */
	if (close(mainfd) == -1) {
		fprintf(stderr, "\n\nclose port: Unable to close %s - %s\n\n", portName, strerror(errno));
		exit(EXIT_FAILURE); // EXIT
	}
	else {
		printf("\n\nclose port: The port %s has been closed correctly.\n\n", portName);
		exit(EXIT_SUCCESS);
	}
}

/*
 * 'config_port2()' - Configure the serial port.
 * Old code from origional code set...
 */
void config_port2() {
	struct termios options;
	
	fcntl(mainfd, F_SETFL, FNDELAY);                  /* Configure port reading */

	/* Get the current options for the port */
	tcgetattr(mainfd, &options);
	cfsetispeed(&options, B9600);                 /* Set the baud rates to 9600 */
	cfsetospeed(&options, B9600);
    
	/* Enable the receiver and set local mode */
	options.c_cflag |= (CLOCAL | CREAD);
	options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |=  CS8;                              /* Select 8 data bits */
	options.c_cflag &= ~CRTSCTS;               /* Disable hardware flow control */  
	
	/* Enable data to be processed as raw input */
	options.c_lflag &= ~(ICANON | ECHO | ISIG);
	
	/* Set the new options for the port */
	tcsetattr(mainfd, TCSANOW, &options);
}

int main (int argc, char *argv[]) {
	if (argc != 2) {
		printf("USAGE: %s PORT. i.e. %s /dev/ttyACM1.\n", argv[0], argv[0]);
		exit(EXIT_FAILURE); // EXIT
	}
	
	mainfd = open_port(argv[1]);
	config_port(mainfd);
	
	printf("Initalizing...\n");
	//neutral(); 	// Send all servo's to their neutral positions.
	waitMS(1000);	// Wait one second.

	int i, j;
	for (i = 0; i < 6; i++) {
		printf("\n\nRound %d\n", i);
		//for (j = 0; j <= 5; j++) {
			//printf("\n\nServo %d\n", j);
			//put(0,3500);	// send servo 0 to position 3500
			put(i,1500);	// send servo 0 to position 3500
			waitMS(1000); // Wait one second.
			//put(0,3500);	// send servo 0 to position 3500
			put(i,5000);	// send servo 0 to position 3500
			waitMS(1000); // Wait one second.
		//}
	}

	// Close the port after use.
	close_port(mainfd, argv[1]);
}

/*
 * This sets up all the servos to their neutral positions.
 */
void neutral() {
	int i;
	for (i = 0; i < 6; i++) {
		put(i, (max + min) / 2);
	}
	printf("All servos initalized correctly\n\n");
}

/*
 * This allows for a set system instruction pause. Note: 1000ms = 1 sec.
 */
void waitMS (int ms) {
	clock_t endwait;
	endwait = clock() + ms * CLOCKS_PER_SEC/1000;
	while (clock() < endwait);
}

void put(int servo, int angle) {
	int buffSize = 6;
	unsigned char buff[buffSize]; // The data to be sebt to the servo controller.
	
	// DWORD is not a standard C datatype.  Typically it represents a double word.
	// See typedef at top
	DWORD len;
	
	unsigned short int temp;
	unsigned char pos_hi, pos_low;
	
	/*
	 * NOTE: max servo positioning is 5500 so in binary this is '0001 1111 1111 1111'
	 * The servo controler takes the angle as two seperate bytes representational of the binary of the angle required.
	 * This is then seperated out into two bytes of data.
	 */
	//temp = angle & 0x1F80; // May not need this if the code below works.
	/*
	 * AND the hex of the desired angle against '0001 1111 1000 0000' i.e get the first 7 bits.
	 * Then bit shift these across for the final hi byte.
	 */
	pos_hi = (angle & 0x1F80) >> 7;	
	/*
	 * AND the hex of the desired angle against '0000 0000 0111 1111' i.e get the last 7 bits.
	 * This produces the final low byte
	 */
	pos_low = angle & 0x7F;
	
	/*
	 *	This uses the Pololu Protocol as specified in the Pololu servo documentation.	
	 */
	/*
	buff[0] = 0xAA;	// start byte
	buff[1] = 0x0C;	// device id
	buff[2] = 0x04;	// command number
	buff[3] = 0x03;	// servo number
	buff[4] = 0x70;	// data1
	buff[5] = 0x2E;	// data2*/

	//buff[0] = 0x84; 		// Command byte: Set Target. 
	//buff[1] = servo; 		//channel; // First data byte holds channel number. 
	//buff[2] = angle & 0x7F; 	// Second byte holds the lower 7 bits of target. 
	//buff[3] = (angle >> 7) & 0x7F;	// Third data byte holds the bits 7-13 of target.
	
	buff[0] = 0xAA;		// start byte
	buff[1] = 0x0C;		// device id
	buff[2] = 0x04;		// command number
	buff[3] = servo;	// servo number
	buff[4] = pos_hi;	// data1
	buff[5] = pos_low;	// data2
	
	/* 
	 * Write data to the serial port.
	 * 	mainfd is the file descriptor of the serial port.
	 * 	buff is a pointer to the data that we want to write to the serial port.
	 *	buffSize is the amount of data that we want to write.
	 */
	int wordsWritten = write(mainfd, &buff, buffSize);

	angles[servo] = angle; // Updates the servo angle list.

	/*
	 * Read data from the serial port.
	 * 	mainfd is the file descriptor of the serial port.
	 * 	buff is a pointer to the array that we want to read into.
	 *	buffSize is the amount of data that we want to read in.
	 */
	int wordsRead = read(mainfd, &chout, sizeof(chout));
 
	printf("write = %d, read = %d\n", wordsWritten, wordsRead);
	printf("Servo %d Set to %d\n", servo, angle);
}

I have tried three different sets of data that i send the servo controller. In all cases the green light flashes to indicate that the servo controller is being communicated with, NOTE that i am using this on a Ubuntu OS so the USB comes up as a /dev/ttyACM0 or ACM1. The green light flashes at a steady rate until data is sent to it using the above code. Then the green light flashes as each command is sent.

The problem comes in that the orange light just keeps flashing at a constant period. I understand that this is where the controller is identifying the Baud rate. I send it the command 0xAA, which i understand sets the baud rate detection. But even though the data is being sent to the USB communicator the orange light still flashes at a regular rate and not blinking to identify that pulses are being sent to the servos.

Could anyone please offer me any ideas or solutions. I have also connected it up to a digital servo but it did not move. I did however connect it to the windows controller and both the orange lights flash and the servo moves correctly.

Any help would be greatly appreciated.

Many thanks in advance.

Paul

That sounds like a cool project! We frequently get requests from people for example C code for controlling the Maestro, so if you could share it with us when you get it working, that would be great.

As for your problem, you need to set the Maestro’s serial mode to USB Dual Port or USB Chained so that it can accept commands from the virtual COM port. This can be done by running UscCmd or the Maestro Control center.

The different serial modes are explained in the Serial Settings section:
pololu.com/docs/0J40/5.a

Did you read the “Writing PC Software to Control the Maestro” section?
pololu.com/docs/0J40/8

–David Grayson

Dear David,

Thank you for your post which was very useful indeed. Have you any idea where i can find the commands to set the Maestro’s serial mode to USB Dual Port or USB Chained so that it can accept commands from the virtual COM port. I can see and send data to both virtual ports but cant seem to find the commands i need to use to set the serial mode. I hope i havent just missed them lol.

I am looking through the dll files and cp files to see if i can find them there.

Once again many thanks for clearing that problem up.

Paul Burton

Hello,

We also frequently respond to those requests: Here is some example code for controlling the Maestro under Ubuntu.

You can use the control center to configure your Maestro as indicated in the documentation; if you need more help with your code after that, you should simplify it to the simplest possible thing that causes the problem - a five-or-six-line program that just sends a single command.

Good luck!
-Paul

How would i set the usb’s serial mode in code and not through the UscCmd or the Maestro Control center? My project has to be completely done in C.

Many thanks

Paul.

Hello,

Why do you need to write code to configure the serial mode? You should only need to do that once, so for most applications, the Control Center is good enough. But if you really need to configure the Maestro from your application, the easiest way is probably to call our command line control utility, UscCmd, from your program.

We also provide a USB SDK that you can download to learn how to access the raw USB commands required to configure the Maestro, then write your own code using the libusb userspace USB library. You can figure it out just by going through the tutorials on the libusb page and reading over our SDK source code, but this way is going to be a lot of work!

-Paul

Thank you for your patience, I think ill try that as it means that as my uni project has to be a completely self contained development i should get more experiance and marks for the extra work lol.

Am i missing something simple in my code. I am completely lost in what to do next to solve my problem. I have coded the write function to transmit the data as described in the controllers document. The green light flickers when the data is being sent and the program confirmes that the data has been written/sent but the orange light does not flicker as it does on the control center you provide but only every second at a steady rate. I can only think that its because i dont turn the servos on individually in my code. The orange light flickers steadly every second but when i use the controle center and i turn one or more servos on the orange light flickers twice at a steady rate.

I have gone into the control center and changed the usb to being chaned and then ran my program but this has not helped. I am now very lost as i have not seen any examples on here which turn on individual servos although this works in the control center and moves the servos so i know its something to do with my code.

Its either my code not turning on a servo, although i dont think you have to or something to do with my OS and maybe the drivers.

Once again im very sorry for the questions but i have found myself stuck at a dead end and dont know how to proceed. Ill have a look at the USBcmd again and the USB SDK you have pointed at to see if this may help.

Thank you for all help and as soon as a solution is found i shall post my final code for all to use.

Paul

Hello,

The Maestro LEDs are described in detail here.

It sounds like you are trying to say that you wrote some code in C, but it does not succeed in moving the servos, even though you can move them with the control center. Is that the case? Did you expect it to turn on a servo? You post does not say what you are trying to do or whether you have any more questions for us.

Anyway, I recommend simplifying your code as much as possible, then posting it here, along with your Maestro settings file and a clear description of what you expect the code to do and what it actually did. The Serial Servo Commands section of the Users Guide might also be helpful to you.

-Paul

I am trying to control a Micro Maestro 6 over USB on Windows XP using C++. I am having the same issues. I have installed the device drivers and can run the Maestro Control Center successfully. When I send down the commands from my code, I do see the green LED blink briefly but the commands are not executed (I have exited the Maestro Control Center prior to running this).

Here is a code snippet:
// set position
iCmdLen = 0;
cCmdBuff[iCmdLen++] = (unsigned char)0xAA;
cCmdBuff[iCmdLen++] = (unsigned char)0x84;
cCmdBuff[iCmdLen++] = (unsigned char)0x01;
cCmdBuff[iCmdLen++] = (unsigned char)0x40;
// alternate position each pass
if (iCmdCount & 1)
cCmdBuff[iCmdLen++] = (unsigned char)0x3e;
else
cCmdBuff[iCmdLen++] = (unsigned char)0x1f;
bWriteSuccess = WriteFile (m_hComPort, cCmdBuff, iCmdLen, &iCmdWrittenLen, NULL);

m_hComPort is a HANDLE that has been successfully opened to the com port shown as the command port in the device manager.

Do I need to be sending these commands through the device driver and if so where is the interface documented?

I have your C# example running but the source for the function Pololu.UsbWrapper.controlTransfer() is not exposed. This may answer my question.

Any help would be appreciated.

el_neutrono, one of the pieces of advice given in this thread was that to send serial commands to the maestro over a virtual COM port you must change the serial mode to USB Chained or USB Dual Port. Have you done that? (Also, you could have saved both of us time by volunteering that information.)

You did not tell us how you know that your Set Target serial command was not executed. What is the difference between the expected behavior and the actual behavior? It’s OK to have the Maestro Control Center open while you are sending serial commands, so you should open the Maestro Control Center and see if any of the sliders in the Status tab change position when you send this command.

You should include the code that defines/assigns all the variables you are using because there might be a problem there (e.g. if you defined cCmdBuff as something other than array of unsigned chars).

The command you are trying to send is not a valid Pololu Protocol or Compact Protocol command. You should get rid of the 0xAA byte at the beginning. Your byte sequence would be OK if your serial mode were “UART, detect baud rate” and sending bytes to the Maestro’s RX line, but that’s not what you’re doing.

It probably doesn’t matter, but what C++ compiler/environment are you using?

What you are trying to do is use the Virtual COM Port interface, not the native USB interface, so the controlTransfer function is not relevant for you. You should be able to send data to the Maestro using CreateFile/WriteFile.

Also, I recommend downloading the Pololu Serial Transmitter utility and using that to send your command to the Maestro. If that works, then we know for sure that the problem lies in your C++ code.

–David

Thanks David,

The solution to my problem was to change the serial mode to the USB Dual Port using the Maestro Control Center. Getting rid of the 0xAA syncing bytes probably helped. I did not need to use the Pololu Serial Transmitter but that would have been a useful tool had my problem continued.