[ros-users] Random and incorrect joystick values in C++ program (Ubuntu Linux OS)
ibwood
ianbenjiman at hotmail.com
Wed Aug 18 20:32:24 UTC 2010
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
More information about the ros-users
mailing list