joyd.cc

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
#include <time.h>
#include <math.h>
#include "robomon_orte.h"

#define JOY_DEV "/dev/input/js0"
#define VMAX 16000

int main(int argc, char *argv[]) 
{
        struct timespec timer;
        int joy_fd, *axis=NULL, num_of_axis=0, num_of_buttons=0, x;
        char *button=NULL, joy_name[80];
        double r=0.15,angle,lenght,v, omega;    
        
        struct js_event js;
        
        orte_function(NULL);
        
        if((joy_fd=open(JOY_DEV, O_NONBLOCK))==-1)
        {
                printf("Unable to open joystick!\n");
                return -1;
        }
        
        ioctl(joy_fd, JSIOCGAXES, &num_of_axis);
        ioctl(joy_fd, JSIOCGBUTTONS, &num_of_buttons);
        ioctl(joy_fd, JSIOCGNAME(80), &joy_name);
        
        printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n", joy_name, num_of_axis, num_of_buttons);
        
        axis = (int *) calloc( num_of_axis, sizeof( int ) );
        button = (char *) calloc( num_of_buttons, sizeof( char ) );
        
        while(1) 
        {
                timer.tv_sec = 0;
                timer.tv_nsec = 2000000;
                
                while(read(joy_fd, &js, sizeof(struct js_event))>0) {           
                        switch (js.type & ~JS_EVENT_INIT)
                        {
                                case JS_EVENT_AXIS:
                                        axis   [ js.number ] = js.value;
                                        break;
                                case JS_EVENT_BUTTON:
                                        button [ js.number ] = js.value;
                                        break;
                        }
                        joy.axisX = axis[0];
                        joy.axisY = axis[1];
                        joy.axisZ = axis[2];
                        joy.axisR = axis[3];
                        joy.axisS1 = axis[4];
                        joy.axisS2 = axis[5];
                        
                        joy.button1 = button[0];
                        joy.button2 = button[1];
                        joy.button3 = button[2];
                        joy.button4 = button[3];
                        joy.button5 = button[4];
                        joy.button6 = button[5];
                        joy.button7 = button[6];
                        joy.button8 = button[7];
                        joy.button9 = button[8];
                        joy.button10 = button[9];
                        joy.button11 = button[10];
                        joy.button12 = button[11];
                        
/*                      v = (double)(axis[0]/32768.0);// * 10000;
                        omega = (double)(axis[1]/32768.0);// * 10000;
                        mspeed.right = -(v + r*omega);
                        mspeed.left = v - r*omega;*/
                        v = (double)axis[1]/32768.0;
                        omega = (double)axis[0]/32768.0;
                        lenght = sqrt( pow(v,2) + pow(omega,2));
                        if (lenght >= 1)                // If lenght si more than 1 is used unit circle
                        {
                                angle = atan2(axis[1], axis[0]);        // computing new speed and omega
                                v = sin(angle);
                                omega = cos(angle);
                        }
                        omega *= 2;
                        mspeed.right = (int)(-((v + r*omega))*VMAX);
                        mspeed.left = (int)(-(v - r*omega)*VMAX);
                        printf("mspeed.left=%d, mspeed.right=%d\n",mspeed.left,mspeed.right);
                        printf("v %f, omega %f\n",v, omega);//#endif
                        printf("length %f, angle %f\n",lenght, angle);//#endif
                        printf("axisX %d, axisY %d\n",axis[0], axis[1]);//#endif
                        ORTEPublicationSend(publisherMotor);                    
                        ORTEPublicationSend(publisherJoy);
                        
                }
                ORTEPublicationSend(publisherMotor);
                ORTEPublicationSend(publisherJoy);
                nanosleep(&timer,NULL);
                
//              printf( "X: %6d  Y: %6d  ", axis[0], axis[1] );
                
/*              if( num_of_axis > 2 )
                        printf("Z: %6d  ", axis[2] );
                        
                if( num_of_axis > 3 )
                        printf("R: %6d  ", axis[3] );
                
                if( num_of_axis > 4 )
                        printf("S1: %6d  ", axis[4] );
                
                if( num_of_axis > 5 )
                        printf("S2: %6d  ", axis[5] );*/
                
                
                        
//              for( x=0 ; x<num_of_buttons ; ++x )
//                      printf("B%d: %d  ", x, button[x] );

                printf("  \r");
                fflush(stdout);
        }
        
        close(joy_fd);
        
        return 0;
}

---