[ros-users] Random and incorrect joystick values in C++ program (Ubuntu Linux OS)
Blaise Gassend
blaise at willowgarage.com
Wed Aug 18 21:50:13 UTC 2010
It is hard to help you much without seeing the data that is actually being
output.
The approach I would take in your shoes is: print more debugging information
until you can isolate the problem.
Print what is coming from the microcontroller, print what you have parsed,
try to dump the microcontroller output from a terminal emulator such as
picocom. This will help you identify whether the microcontroller is sending
you bad data, whether you are reading the serial port incorrectly, and
If they all look bad, then the microcontroller is probably sending you bad
data. Otherwise you might discover that your parsing is bad, or your serial
port reading.
Are you sure your port rate setting is good. I have had trouble in the past
using stty. Make life easier for yourself and set the baud rate directly
from code http://www.captain.at/howto-simple-serial-port-test-example.php
Is there any chance multiple instances of your program are running at once,
or could some other program be reading the same port? Since you have no
locking, they will be racing for the data, and each getting a subset of it.
That could certainly lead to garbled results. Use lsof to see who has the
port open.
On Wed, Aug 18, 2010 at 1:32 PM, ibwood <ianbenjiman at hotmail.com> wrote:
>
> So, let me try to explain this the best I can..
>
> I am running a C++ program where a microcontroller accepts joystick values
> and converts them to hexadecimal in order to communicate with motor drivers
> installed on my robot. The string that the joystick outputs is "x-axis val
> (1-3 digits), y-axis val (1-3 digits), z-axis val (1-3 digits), deadman
> switch (1 digit; 0 or 1)". When I run the node and am not controlling the
> joystick at all, all three axes should read 127 (neutral; values are
> between
> 0 and 255 from the joystick), but they don't. They are completely random.
>
> I am able to observe changes in the string's values when I use the
> joystick,
> which is good, it means the joystick is connected properly. However, I
> can't
> for the life of me figure out why I am receiving random values
> simultaneously.. we're talking a value difference of 100 at times. Has
> anybody had experience with joystick and motor controller programming who
> can help me out? I would sincerely appreciate it.
>
> Here is my code:
>
>
> ____________________________________________________________________________________________________
> #include "ros/ros.h"
> #include "std_msgs/String.h"
>
> #include <sstream>
> #include <cmath>
> #include <iostream>
>
> #include <sys/ioctl.h>
> #include <sys/types.h>
> #include <sys/stat.h>
> #include <stdio.h>
> #include <limits.h>
> #include <string.h>
> #include <fcntl.h>
> #include <errno.h>
> #include <termios.h>
> #include <unistd.h>
>
> #define PI 3.14159265358979323846264338327950288419716
>
> const double CIRC=0.7853981633974483, R=0.55, RATIO=127; //
> initialize
> constants
>
> int main(int argc, char **argv)
> {
> //declare all variables
> int ms1, ms2, ms3, ms4, omnibotPoseInt[4];
> int i=0, j=0, k=0, wr, rd, x=0, y=0, z=0, fd0, fd1, fd2, numSent=0;
> char parseChar[1], stringIn[50], mc_char, *temp;
>
>
> //set settings for HCS12 microcontroller
>
> system("stty -F /dev/ttyUSB0 115200 cs8 -cstopb -parity -icanon
> hupcl
> -crtscts min 1 time 1");
>
>
> //attempt to open serial port connected to the microprocessor
>
> fd0 = open("/dev/ttyUSB0",O_RDWR | O_NOCTTY | O_NDELAY);
>
>
> //check for errors opening port
>
> if (fd0 == -1 )
>
> {
>
> printf("open_port: Unable to open /dev/HCS12\n");
>
> }
>
>
> else //if no error
>
> {
>
> fcntl(fd0,F_SETFL,0);
>
> printf("Test Port HCS12 has been successfully opened and %d
> is the file
> description\n",fd0);
>
> }
>
> //settings for motor driver m1 & m2
> system("stty -F /dev/ttyUSB1 9600 cs8 -cstopb -parity -icanon hupcl
> -crtscts min 1 time 1");
>
> //open serial port connected to motor driver for motors 1 and 2
> fd1 = open("/dev/ttyUSB1",O_RDWR | O_NOCTTY | O_NDELAY);
>
> if (fd1 == -1 )
>
> {
>
> perror("open_port: Unable to open /dev/Driver12\n");
>
> }
>
> else // if no error
>
> {
>
> fcntl(fd1,F_SETFL,0);
>
> printf("Test Port Driver12 has been successfully opened and
> %d is the file
> description\n",fd1);
>
> }
>
> //settings for motor driver m3 & m4
> system("stty -F /dev/ttyUSB2 9600 cs8 -cstopb -parity -icanon hupcl
> -crtscts min 1 time 1");
>
> //open serial port connected to motor driver for motors 3 and 4
> fd2 = open("/dev/ttyUSB2",O_RDWR | O_NOCTTY | O_NDELAY);
>
> if (fd2 == -1 )
>
> {
>
> perror("open_port: Unable to open /dev/Driver34\n");
>
> }
>
> else // if no error
>
> {
>
> fcntl(fd2,F_SETFL,0);
>
> printf("Test Port Driver34 has been successfully opened and
> %d is the file
> description\n",fd2);
>
> }
>
> // initalize node
> ros::init(argc, argv, "joyTalker");
>
> ros::NodeHandle joyTalker;
>
> // Publish to topic joyChatter
> ros::Publisher pub =
> joyTalker.advertise<std_msgs::String>("joyChatter",
> 1000);
>
> ros::Rate r(10);
>
> while (joyTalker.ok())
> {
> parseChar[0]=0;
>
> for(j=0; j<50; j++)
> {
> stringIn[j]=0;
> //initialize the string
> }
>
> i=0;
>
> while(parseChar[0] != 10)
> {
> rd=read(fd0,parseChar,1);
> //printf("%s\n",parseChar);
> if ((parseChar[0] > 43 && parseChar[0] < 58) ||
> (parseChar[0]==32))
> {
> stringIn[i]=parseChar[0];
> i++;
> }
> }
>
> // Parse the msg from the node
> temp = strtok(stringIn,",");
>
> k=0;
>
> while(temp != NULL)
> {
> omnibotPoseInt[k] = atoi(temp);
> temp = strtok(NULL,",");
> k++;
> }
>
> //conversion to valid motor speeds
> x = omnibotPoseInt[0]-RATIO;
> y = omnibotPoseInt[1]-RATIO;
> z = omnibotPoseInt[2]-RATIO;
>
> printf("omnibotPoseInt[0]: %d, omnibotPoseInt[1]: %d,
> omnibotPoseInt[2]:
> %d\n", omnibotPoseInt[0], omnibotPoseInt[1], omnibotPoseInt[2]);
>
> //kinematics of the omnimaxbot according to mecanum wheels
> ms1 = ((x/sqrt(2))+y+(R*tan((z/RATIO)*(2*PI))))/CIRC;
> ms2 = (-(x/sqrt(2))+y+(R*tan((z/RATIO)*(2*PI))))/CIRC;
> ms3 = ((x/sqrt(2))+y-(R*tan((z/RATIO)*(2*PI))))/CIRC;
> ms4 = (-(x/sqrt(2))+y-(R*tan((z/RATIO)*(2*PI))))/CIRC;
>
> //motor controller language, specified by an exclamation
> mark, a character
> (a or A is channel 1, b or B is channel 2), and then the speed
>
> if(ms1<0) //Motor Controller
> 1, Channel 1, Motor 1
> {
> ms1=-ms1;
> mc_char = 'a'; //move backwards
> }
> else
> {
> mc_char = 'A'; //move forwards
> }
>
> //write/send data to the specified port
> numSent = sprintf(stringIn, "!%c%2X", mc_char, ms1);
> wr = write(fd1,stringIn,numSent);
>
> if(ms2<0) //Motor Controller
> 1, Channel 2, Motor 2
> {
> ms2=-ms2;
> mc_char = 'b'; //move backwards
> }
> else
> {
> mc_char = 'B'; //move forwards
> }
>
> numSent = sprintf(stringIn, "!%c%2X", mc_char, ms2);
> wr = write(fd1,stringIn,numSent);
>
> if(ms3<0) //Motor Controller
> 2, Channel 1, Motor 3
> {
> ms3=-ms3;
> mc_char = 'a';
> }
> else
> {
> mc_char = 'A';
> }
>
> numSent = sprintf(stringIn, "!%c%2X", mc_char, ms3);
> wr = write(fd2,stringIn,numSent);
>
> if(ms4<0) //Motor Controller
> 2, Channel 2, Motor 4
> {
> ms4=-ms4;
> mc_char = 'b';
> }
> else
> {
> mc_char = 'B';
> }
>
> numSent = sprintf(stringIn, "!%c%2X", mc_char, ms4);
> wr = write(fd2,stringIn,numSent);
>
> printf("got this string %s\n",stringIn);
>
> // convert message to string for ROS node communication
> std_msgs::String msg;
> std::stringstream ss;
> ss << stringIn;
> ROS_INFO("%s", ss.str().c_str());
> msg.data = ss.str();
> // publish data under topic joyChatter
> pub.publish(msg);
>
> ros::spinOnce();
> }
>
> return 0;
> }
>
> ____________________________________________________________________________________________________
> --
> View this message in context:
> http://ros-users.122217.n3.nabble.com/Random-and-incorrect-joystick-values-in-C-program-Ubuntu-Linux-OS-tp1213793p1213793.html
> Sent from the ROS-Users mailing list archive at Nabble.com.
>
>
> ------------------------------------------------------------------------------
> This SF.net email is sponsored by
>
> Make an app they can't live without
> Enter the BlackBerry Developer Challenge
> http://p.sf.net/sfu/RIM-dev2dev
> _______________________________________________
> ros-users mailing list
> ros-users at lists.sourceforge.net
> https://lists.sourceforge.net/lists/listinfo/ros-users
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20100818/c66d2628/attachment-0003.html>
More information about the ros-users
mailing list