I am still learning how to control servos using my Maestro 6 ch USB controller. I successfully installed and controlled servos with the Windows Maestro controller to start with. I was also successful with the LINUX drivers and was able to control servos using the ‘UscCmd’ command line tool and also the sample bash script in the user guide.
However, when I try to run the user guide sample C code in Linux (Debian Squeeze), it controls the servo ok, but throws errors when reading the servo position ONLY when the servo is at 5000. If I change the script to switch between 4800/7000 instead of 5000/7000, everything is ok.
Here is the code and sample output with 5000/7000 (errors), then 4800/7000 (no errors). I am compiling with ‘cc -o outfile samplecode.c’. I also tried with gcc with same results.
Any insight would be appreciated. Thanks.
Steve
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#ifdef _WIN32
#define O_NOCTTY 0
#else
#include <termios.h>
#endif
// Gets the position of a Maestro channel.
// See the "Serial Servo Commands" section of the user's guide.
int maestroGetPosition(int fd, unsigned char channel)
{
unsigned char command[] = {0x90, channel};
if(write(fd, command, sizeof(command)) == -1)
{
perror("Error writing");
return -1;
}
unsigned char response[2];
if(read(fd,response,2) != 2)
{
// printf("response[0] = %d\n", response[0]);
// printf("response[1] = %d\n", response[1]);
perror("Error reading");
return -1;
}
return response[0] + 256*response[1];
}
// Sets the target of a Maestro channel.
// See the "Serial Servo Commands" section of the user's guide.
// The units of 'target' are quarter-microseconds.
int maestroSetTarget(int fd, unsigned char channel, unsigned short target)
{
unsigned char command[] = {0x84, channel, target & 0x7F, target >> 7 & 0x7F};
if (write(fd, command, sizeof(command)) == -1)
{
perror("error writing");
return -1;
}
return 0;
}
int main()
{
// Open the Maestro's virtual COM port.
const char * device = "/dev/ttyACM0"; // Linux
int fd = open(device, O_RDWR | O_NOCTTY);
if (fd == -1)
{
perror(device);
return 1;
}
#ifndef _WIN32
struct termios options;
tcgetattr(fd, &options);
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
options.c_oflag &= ~(ONLCR | OCRNL);
tcsetattr(fd, TCSANOW, &options);
#endif
int position = maestroGetPosition(fd, 0);
printf("\n***************************************\n");
printf("Current position is %d.\n", position);
int target = (position < 6000) ? 7000 : 5000;
printf("Setting target to %d (%d us).\n", target, target/4);
printf("***************************************\n\n");
maestroSetTarget(fd, 0, target);
close(fd);
return 0;
}
------------------CUT HERE ----------------------------
ERRORED OUTPUT BELOW (5000/7000)
========================================
***************************************
Current position is 7000.
Setting target to 5000 (1250 us).
***************************************
# ./servo_cntl
Error reading: Success
***************************************
Current position is -1.
Setting target to 7000 (1750 us).
***************************************
# ./servo_cntl
***************************************
Current position is 7000.
Setting target to 5000 (1250 us).
***************************************
# ./servo_cntl
Error reading: Success
***************************************
Current position is -1.
Setting target to 7000 (1750 us).
***************************************
WHEN SWITCING BETWEEN 4800 and 7000, everything is ok
======================================================
./servo_cntl
***************************************
Current position is 7000.
Setting target to 4800 (1200 us).
***************************************
./servo_cntl
***************************************
Current position is 4800.
Setting target to 7000 (1750 us).
***************************************
# ./servo_cntl
***************************************
Current position is 7000.
Setting target to 4800 (1200 us).
***************************************