src/joyd/joyd.cc

   1 /*
   2  * joyd.cc                      07/06/01
   3  *
   4  * Joystick daemon.
   5  *
   6  * Copyright: (c) 2007 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13 #include <unistd.h>
  14 #include <string.h>
  15 #include <fcntl.h>
  16 #include <sys/ioctl.h>
  17 #include <linux/joystick.h>
  18 #include <time.h>
  19 #include <math.h>
  20 #include <signal.h>
  21 #include <roboorte_robottype.h>
  22 #include <actuators.h>
  23 #include "joyd.h"
  24
  25 #define JOY_DEV "/dev/input/js0"
  26 #define VMAX 16000
  27 #define JOY_NAME "Logitech Logitech Extreme 3D"
  28
  29 struct robottype_orte_data orte;
  30 int joy_fd;
  31 static int *axis_prev, *axis;
  32 static char *buttons_prev, *buttons;
  33
  34 static void set_robot_speed(void)
  35 {
  36         double r=0.15; //FIXME use radius from robodim?
  37         double angle = 0, lenght, v, omega;
  38
  39         // printf("axis x = %d, axis y = %d\n",axis[AXIS_X], axis[AXIS_Y]);
  40         v = (double)axis[AXIS_Y]/32768.0;
  41         omega = (double)axis[AXIS_X]/32768.0;
  42         lenght = sqrt( pow(v,2) + pow(omega,2));
  43         /* if lenght si more than 1 is used unit circle */
  44         if (lenght >= 1) {
  45                 /* computing new speed and omega */
  46                 angle = atan2(axis[AXIS_Y], axis[AXIS_X]);
  47                 v = sin(angle);
  48                 omega = cos(angle);
  49         }
  50         omega *= 2;
  51
  52         orte.motion_speed.right = (int)(-((v + r*omega))*VMAX);
  53         orte.motion_speed.left = (int)(-(v - r*omega)*VMAX);
  54         ORTEPublicationSend(orte.publication_motion_speed);
  55         printf("Publishing commands: right %d left %d\n", orte.motion_speed.right, orte.motion_speed.left);
  56 }
  57
  58 /* ************************************************** */
  59
  60 static void button_act(char state, int id)
  61 {
  62         switch(id) {
  63                 case BT1:
  64                         if(state) {
  65                                 //act ON
  66                         } else {
  67                                 ;//act OFF
  68                         }
  69                         break;
  70                 case BT2:
  71                         if(state) {
  72                                 //act ON
  73                         } else {
  74                                 ;//act OFF
  75                         }
  76                         break;
  77                 case BT3:
  78                         if(state)
  79                                 ;//act ON
  80                         else
  81                                 ;//act OFF
  82                         break;
  83                 case BT4:
  84                         if(state) {
  85                                 ;//act ON
  86                         } else
  87                                 ;//act OFF
  88                         break;
  89                 case BT5:
  90                         if(state)
  91                                 ;//act ON
  92                         else
  93                                 ;//act OFF
  94                         break;
  95                 case BT6:
  96                         if(state) {
  97                                 ;//act ON
  98                         } else
  99                                 ;//act OFF
 100                         break;
 101                 case BT7:
 102                         if(state) {
 103                                 ; //act ON
 104                         } else
 105                                 ;//act OFF
 106                         break;
 107                 case BT8:
 108                         if(state)
 109                                 ;//act ON
 110                         else
 111                                 ;//act OFF
 112                         break;
 113                 case BT9:
 114                         if(state) {
 115                                 ; //act ON
 116                         } else
 117                                 ;//act OFF
 118                         break;
 119                 case BT10:
 120                         if(state)
 121                                 ;//act ON
 122                         else
 123                                 ;//act OFF
 124                         break;
 125                 case BT11:
 126                         if(state)
 127                                 ; //act ON
 128                         else
 129                                 ;//act OFF
 130                         break;
 131                 case BT12:
 132                         if(state)
 133                                 ;//act ON
 134                         else
 135                                 ;//act OFF
 136                         break;
 137                 default:
 138                         printf("Unknown button pressed!\n");
 139                         break;
 140         }
 141 }
 142
 143 static void process_axis(int value, int id)
 144 {
 145         switch(id) {
 146                 case AXIS_X:
 147                 case AXIS_Y:
 148                         set_robot_speed();
 149                         break;
 150                 case AXIS_Z:
 151                         break;
 152                 case AXIS_R:
 153                         break;
 154                 case AXIS_S1:
 155                         switch (value) {
 156                         case 32767:
 157                                 printf("jaws CLOSE\n");
 158                                 break;
 159                         case -32767:
 160                                 printf("jaws OPEN\n");
 161                                 break;
 162                         case 0:
 163                                 printf("jaws stop\n");
 164                                 break;
 165                         default:
 166                                 printf("error!\n");
 167                                 break;
 168                         }
 169                         break;
 170                 case AXIS_S2:
 171                         switch (value) {
 172                         case 32767:
 173                                 act_lift(40000, 0);
 174                                 printf("lift DOWN\n");
 175                                 break;
 176                         case -32767:
 177                                 act_lift(2000, 0);
 178                                 printf("lift UP\n");
 179                                 break;
 180                         case 0:
 181                                 printf("lift stop\n");
 182                                 break;
 183                         default:
 184                                 printf("error!\n");
 185                                 break;
 186                         }
 187                         break;
 188                 default:
 189                         printf("Unknown axis changed!\n");
 190                         break;
 191         }
 192 }
 193
 194 static void process_button(char state, int id)
 195 {
 196         if(state == 1) {
 197                 if(buttons_prev[id] != 1) {
 198                         state = ~state;
 199                         buttons_prev[id] = 1;
 200                 }
 201                 button_act(state, id);
 202         }
 203         else {
 204                 buttons_prev[id] = 0;
 205                 button_act(state, id);
 206         }
 207 }
 208
 209 static void int_handler(int sig)
 210 {
 211         printf("joyd stopping.\n");
 212
 213         /* orte domain destroy */
 214         robottype_roboorte_destroy(&orte);
 215
 216         /* close file descriptor */
 217         close(joy_fd);
 218
 219         exit(0);
 220 }
 221
 222 int open_joystick(char *name, int *num_of_axis, int *num_of_buttons)
 223 {
 224         int ret;
 225
 226         joy_fd = -1;
 227
 228         if((joy_fd=open("/dev/input/js0", O_NONBLOCK))==-1) {
 229                 printf("No joystick detected!\n");
 230                 goto ret;
 231         }
 232
 233         ioctl(joy_fd, JSIOCGNAME(80), name);
 234         ret = strcmp(JOY_NAME, name);
 235         if(ret != 0)
 236                 printf("Logitech joystick not present as js0\n");
 237         else {
 238                 ioctl(joy_fd, JSIOCGAXES, num_of_axis);
 239                 ioctl(joy_fd, JSIOCGBUTTONS, num_of_buttons);
 240                 goto ret;
 241         }
 242
 243         if((joy_fd=open("/dev/input/js1", O_NONBLOCK))==-1) {
 244                 printf("No joystick detected!\n");
 245                 goto ret;
 246         }
 247
 248         ioctl(joy_fd, JSIOCGNAME(80), name);
 249         ret = strcmp(JOY_NAME, name);
 250         if(ret != 0)
 251                 printf("Logitech joystick not present as js1\n");
 252         else {
 253                 ioctl(joy_fd, JSIOCGAXES, num_of_axis);
 254                 ioctl(joy_fd, JSIOCGBUTTONS, num_of_buttons);
 255                 goto ret;
 256         }
 257
 258 ret:
 259         return joy_fd;
 260 }
 261
 262 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
 263                         void *sendCallBackParam)
 264 {
 265 }
 266
 267 int main(int argc, char *argv[])
 268 {
 269         struct timespec timer;
 270         int num_of_axis=0, num_of_buttons=0;
 271         char joy_name[80];
 272         struct js_event js;
 273
 274         signal(SIGINT, int_handler);
 275
 276         orte.strength = 30;
 277
 278         /* orte initialization */
 279         if (robottype_roboorte_init(&orte)) {
 280                 printf("Unable to initialize ORTE.\n");
 281                 exit(1);
 282         }
 283
 284         act_init(&orte);
 285
 286         /* creating publishers */
 287         robottype_publisher_motion_speed_create(&orte, send_dummy_cb, &orte);
 288         robottype_publisher_lift_cmd_create(&orte, send_dummy_cb, &orte);
 289
 290         if(open_joystick(joy_name, &num_of_axis, &num_of_buttons) == -1) {
 291                 printf("Joystick not found, exiting...\n");
 292                 return -1;
 293         }
 294
 295         printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n",
 296                                 joy_name, num_of_axis, num_of_buttons);
 297
 298         axis = (int *) calloc( num_of_axis, sizeof( int ) );
 299         buttons = (char *) calloc( num_of_buttons, sizeof( char ) );
 300         axis_prev = (int *) calloc( num_of_axis, sizeof( int ) );
 301         buttons_prev = (char *) calloc( num_of_buttons, sizeof( char ) );
 302
 303         while(1) {
 304                 timer.tv_sec = 0;
 305                 timer.tv_nsec = 100000000;
 306
 307                 while(read(joy_fd, &js, sizeof(struct js_event))>0) {
 308                         switch (js.type & ~JS_EVENT_INIT) {
 309                                 case JS_EVENT_AXIS:
 310                                         axis   [ js.number ] = js.value;
 311                                         process_axis(js.value, js.number);
 312                                         break;
 313                                 case JS_EVENT_BUTTON:
 314                                         process_button(js.value, js.number);
 315                                         buttons [ js.number ] = js.value;
 316                                         break;
 317                                 default:
 318                                         break;
 319                         }
 320
 321                 }
 322                 nanosleep(&timer,NULL);
 323         }
 324
 325         return 0;
 326 }