Hi,
I try to make a code in C Linux for the servo Controller.
Ma hard Linux is a FOXBOARD (http://www.acmesystems.it/).
But controller don’t work
My code:
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <stdarg.h>
#include <signal.h>
struct termios stdin_saved_attributes;
struct termios tty_saved_attributes;
int tty_fd;
int tty_open(char* tty_dev) {
struct termios new_attributes;
//tty_fd = open(tty_dev,O_RDWR| O_NOCTTY | O_NONBLOCK);
tty_fd = open(tty_dev,O_RDWR| O_NOCTTY);
if (tty_fd<0) {
return -1;
} else {
tcgetattr(tty_fd,&tty_saved_attributes);
tcgetattr(tty_fd,&new_attributes);
// Set the new attributes for the serial port
// http://linux.about.com/library/cmd/blcmdl3_termios.htm
// http://www.gnu.org/software/libc/manual/html_node/Low_002dLevel-I_002fO.html#Low_002dLevel-I_002fO
// c_cflag
// new_attributes.c_cflag |= CREAD; // Enable receiver
new_attributes.c_cflag |= (CLOCAL | CREAD); // Enable receiver
new_attributes.c_cflag |= B9600; // Set baud rate
// new_attributes.c_cflag |= CS8; // 8 data bit
new_attributes.c_cflag &= ~PARENB; /* 8N1 */
new_attributes.c_cflag &= ~CSTOPB;
new_attributes.c_cflag &= ~CSIZE;
new_attributes.c_cflag |= CS8;
new_attributes.c_iflag |= CRTSCTS; /* RTS/CTS */
new_attributes.c_iflag |= (IXON | IXOFF | IXANY); /* Xon/Xoff */
/*
// c_iflag
new_attributes.c_iflag |= IGNPAR; // Ignore framing errors and parity errors.
// c_lflag
new_attributes.c_lflag &= ~(ICANON); // DISABLE canonical mode.
// Disables the special characters EOF, EOL, EOL2,
// ERASE, KILL, LNEXT, REPRINT, STATUS, and WERASE, and buffers by lines.
new_attributes.c_lflag &= ~(ECHO); // DISABLE this: Echo input characters.
new_attributes.c_lflag &= ~(ECHOE); // DISABLE this: If ICANON is also set, the ERASE character erases the preceding input
// character, and WERASE erases the preceding word.
new_attributes.c_lflag &= ~(ISIG); // DISABLE this: When any of the characters INTR, QUIT, SUSP,
*/ // or DSUSP are received, generate the corresponding signal.
//new_attributes.c_cc[VMIN]=1; // Minimum number of characters for non-canonical read.
//new_attributes.c_cc[VTIME]=0; // Timeout in deciseconds for non-canonical read.
(void)cfsetispeed(&new_attributes, B9600);
(void)cfsetospeed(&new_attributes, B9600);
tcsetattr(tty_fd, TCSANOW, &new_attributes);
}
return tty_fd;
}
// Serial version of printf
void tty_printf(char *format, ...) {
va_list argptr;
char buffer[256] = {0};
va_start(argptr,format);
vsprintf(buffer,format,argptr);
va_end(argptr);
write(tty_fd,buffer,strlen(buffer));
}
void termination_handler (int signum) {
tcsetattr(STDIN_FILENO,TCSANOW,&stdin_saved_attributes);
if (tty_fd>0) tcsetattr (tty_fd,TCSANOW,&tty_saved_attributes);
close(tty_fd);
printf("Exit\n");
exit(0);
}
int stdin_init(void) {
struct termios tattr;
// Make sure stdin is a terminal
if (!isatty (STDIN_FILENO)) {
fprintf (stderr,"stdin is not a terminal\n");
return -1;
}
// Save the terminal attributes so we can restore them later.
tcgetattr (STDIN_FILENO, &stdin_saved_attributes);
// Set the funny terminal modes.
tcgetattr (STDIN_FILENO, &tattr);
tattr.c_lflag &= ~(ICANON | ECHO); /* Clear ICANON and ECHO. */
tattr.c_cc[VMIN] = 0;
tattr.c_cc[VTIME] = 0;
tcsetattr (STDIN_FILENO, TCSAFLUSH, &tattr);
return 0;
}
int main(int argc, char *argv[]) {
char rxChar;
char txChar;
printf("Pololu Test (press ctrl-c to exit)\n");
if (tty_open("/dev/ttyS3")<0) {
fprintf (stderr,"tty open error %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (stdin_init()<0) {
printf("stdin init error %s\n", strerror(errno));
exit(EXIT_FAILURE);
}
if (signal (SIGINT, termination_handler) == SIG_IGN) signal (SIGINT, SIG_IGN);
if (signal (SIGHUP, termination_handler) == SIG_IGN) signal (SIGHUP, SIG_IGN);
if (signal (SIGTERM, termination_handler) == SIG_IGN) signal (SIGTERM, SIG_IGN);
unsigned char buf[3];
buf[0]=0x80;
buf[1]=0x02;
buf[2]=0x00;
write(tty_fd,buf,3);
//fflush(stdout);
while (1) {
buf[0]=0xFF;
buf[1]=0x01;
buf[2]=0xFE;
write(tty_fd,buf,3);
}
return EXIT_SUCCESS;
}
Can you help please ?
Thank you
Olivier