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