write() C function to Servo Controller in Linux


7 posts
by LuxManix » Thu Jan 03, 2013 12:44 am
Hello folks,

I have once again stumbled upon a problem which I have been trying to solve for a good amount of time but to no avail.. I am currently trying to control the Parallax Propeller Servo Controller via Debian Linux on the Raspberry Pi.
User manual for the controller: http://www.parallax.com/Portals/0/Downl ... e-v1.1.pdf
I have done all of the nitty gritty work of opening up a virtual serial port and being able to read from the device but I am having major problems writing commands to the controller.

It all basically comes down to one function which in C is write(). The Servo Controller requires 8 bytes of information to set a position of a servo so I need the following format

Syntax: “!SC” <channel> <ramp speed> <lowbyte> <highbyte> <CR>
Reply: None


So using the write C command which is in the following format:

#include <fcntl.h>
int write( int handle, void *buffer, int nbyte );


I have successfully passed "CHARACTERS" to the servo controlller. (ex. !SCVER?) However when I try to send a mix of characters AND integers I come across problems, the servo controller does not respond to my commands.

Here is my command
n = write(tty_fd,("!SC",&i,&i,&lowbyte,&highbyte,"\r"),8);


I do not think this is in the correct format.... Could anyone lend a helping hand in helping me solve this problem? I tried googling for hours but with little luck. The mix of characters and integers is what seems to be the problem. I would send all 8 bytes of characters instead of mixing integers but since decimals 0-31 are not typical characters, writing these values is not easy. I also tried splitting up the write command into first sending the characters !SC and then the integers separately and then the return carriage but that did not work either....

I have also tried sending all hex values using this code but it did not work also:
Code: Select all
        char  buf[8] = {0x21,0x53,0x43,0x00,0x00,0x20,0x03,'\r'}; //!SC 0 0 32 3 \r
        n = write(tty_fd,buf,8);


I have also seen Gordons WiringPi Serial Library but it only has commands that send one character out at a time and I need to send 8 bytes..

Many Thanks!
Robert

Here is the entire code:
Code: Select all
#include <string.h>#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <wiringSerial.h>
 
int main(int argc,char** argv)
{
        struct termios tio;
        struct termios stdio;
        struct termios old_stdio;
        int tty_fd;
 
        unsigned char c='D';
        tcgetattr(STDOUT_FILENO,&old_stdio);
 
        memset(&stdio,0,sizeof(stdio));
        stdio.c_iflag=0;
        stdio.c_oflag=0;
        stdio.c_cflag=0;
        stdio.c_lflag=0;
        stdio.c_cc[VMIN]=1;
        stdio.c_cc[VTIME]=0;
        tcsetattr(STDOUT_FILENO,TCSANOW,&stdio);
        tcsetattr(STDOUT_FILENO,TCSAFLUSH,&stdio);
        fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);       // make the reads non-blocking
 
        memset(&tio,0,sizeof(tio));
        tio.c_iflag=0;
        tio.c_oflag=0;
        tio.c_cflag=CS8|CREAD|CLOCAL;           // 8n1, see termios.h for more information
        tio.c_lflag=0;
        tio.c_cc[VMIN]=1;
        tio.c_cc[VTIME]=5;
       


        tty_fd=open("/dev/ttyUSB0", O_RDWR | O_NONBLOCK);     
        cfsetospeed(&tio,B2400);            // 115200 baud
        cfsetispeed(&tio,B2400);            // 115200 baud


        tcsetattr(tty_fd,TCSANOW,&tio);


        int count = 0;
        int n=0;
        unsigned char buffer[8];
        //FIND LOWBYTE AND HIGHBYTE BASED ON INPUTED SPEED
        int speed = 800;
        printf("speed: %d\n\r", speed);
        int highbyte = speed >> 8;
        printf("highbyte: %d\n\r", highbyte);
        highbyte = highbyte << 8;
        int lowbyte = speed-highbyte;
        printf("lowbyte: %d\n\r", lowbyte);

        n = write(tty_fd,("!SC",&i,&i,&lowbyte,&highbyte,"\r"),8);
        if (n < 0)
            fputs("write() of 8 bytes failed!\n\r", stderr);
        else
            printf ("Write succeed n = %i\n\r", n );
 
        close(tty_fd);
        tcsetattr(STDOUT_FILENO,TCSANOW,&old_stdio);
 
        return EXIT_SUCCESS;
}
Posts: 10
Joined: Tue Jul 31, 2012 5:30 am
by joan » Thu Jan 03, 2013 1:07 am
Declare a char cmdBuf[9]

sprintf(cmdbuf, "!SC%c%c%c%c\r", channel, ramp_speed, lowbyte, highbyte);
n = write(tty_fd, cmdBuf,8);
User avatar
Posts: 4114
Joined: Thu Jul 05, 2012 5:09 pm
Location: UK
by LuxManix » Thu Jan 03, 2013 1:36 am
is the cmdBuf different from the cmdbuf?

I tried adding the following lines to no avail..
Code: Select all
        char cmdBuf[8];
        sprintf(cmdBuf, "!SC%c%c%c%c\r", 0, 0, lowbyte, highbyte);
        n = write(tty_fd,cmdBuf,8);
Posts: 10
Joined: Tue Jul 31, 2012 5:30 am
by joan » Thu Jan 03, 2013 1:39 am
It doesn't matter what the variable is as long as it's consistent. I meant to use the name cmdBuf.

Your original posted code appeared to have syntax errors. Did it compile/link?

cmdBuf[9] is needed, one for the terminator inserted by sprintf.
User avatar
Posts: 4114
Joined: Thu Jul 05, 2012 5:09 pm
Location: UK
by jojopi » Thu Jan 03, 2013 2:07 am
LuxManix wrote:n = write(tty_fd,("!SC",&i,&i,&lowbyte,&highbyte,"\r"),8);
In a context where only one value is wanted, the comma operator discards the values on the left and returns the one on the right. So this is just write(tty_fd, "\r", 8) and it is trying to send 8 bytes from a buffer that contains only two. By the way, you should be compiling with "gcc -Wall", which will produce the warnings "left-hand operand of comma expression has no effect".
char buf[8] = {0x21,0x53,0x43,0x00,0x00,0x20,0x03,'\r'}; //!SC 0 0 32 3 \r
n = write(tty_fd,buf,8);
That is basically correct and sends exactly the bytes you expect. (Though if the data vary at run time you would be better to use the sprintf() method to populate the buffer.) What makes you think it is not working?
I have also seen Gordons WiringPi Serial Library but it only has commands that send one character out at a time and I need to send 8 bytes..
You can just call it eight times. (This is actually more convenient since you did not have the data in a single buffer anyway.) If you put the characters within (10/baud) seconds the device cannot tell the difference, and it should not care anyway.
User avatar
Posts: 1859
Joined: Tue Oct 11, 2011 8:38 pm
by LuxManix » Thu Jan 03, 2013 6:01 pm
Excellent. Thank you for the help! Both methods worked. For those interested here is the final segment of code that controls motor position:

Code: Select all
            int speed=500;
            highbyte = ((speed) >> 8) & 0xFF;                           //FIND LOWBYTE AND HIGHBYTE BASED ON INPUTED SPEED
            lowbyte = (speed) & 0xFF;
            position = {0x21,0x53,0x43,channel,ramp_speed,lowbyte,highbyte,'\r'};    //Set Position COMMAND! !SC (channel) (ramp) (lowbyte) (highbyte)
            write(tty_fd,position,8); 
Posts: 10
Joined: Tue Jul 31, 2012 5:30 am
by gordon@drogon.net » Thu Jan 03, 2013 10:18 pm
LuxManix wrote:
I have also seen Gordons WiringPi Serial Library but it only has commands that send one character out at a time and I need to send 8 bytes..


The wiringSerial library includes: serialPuts (char *str); and serialPrintf (char *fmt, ...);

However the value returned from serialOpen() is the standard linux file descriptor, so you can use write(fd, buf, n)

if you like.

-Gordon
--
Gordons projects: https://projects.drogon.net/
User avatar
Posts: 1421
Joined: Tue Feb 07, 2012 2:14 pm
Location: Devon, UK