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                                 act_jaws(CATCH);
  67                                 printf("jaws CATCH\n");
  68                         } else {
  69                                 ;//act OFF
  70                         }
  71                         break;
  72                 case BT2:
  73                         if(state) {
  74                                 act_lift(0, 0, 1);
  75                                 //act ON
  76                         } else {
  77                                 act_lift(0, 0, 0);
  78                                 ;//act OFF
  79                         }
  80                         break;
  81                 case BT3:
  82                         if(state)
  83                                 ;//act ON
  84                         else
  85                                 ;//act OFF
  86                         break;
  87                 case BT4:
  88                         if(state) {
  89                                 ;//act ON
  90                         } else
  91                                 ;//act OFF
  92                         break;
  93                 case BT5:
  94                         if(state)
  95                                 ;//act ON
  96                         else
  97                                 ;//act OFF
  98                         break;
  99                 case BT6:
 100                         if(state) {
 101                                 ;//act ON
 102                         } else
 103                                 ;//act OFF
 104                         break;
 105                 case BT7:
 106                         if(state) {
 107                                 ; //act ON
 108                         } else
 109                                 ;//act OFF
 110                         break;
 111                 case BT8:
 112                         if(state)
 113                                 ;//act ON
 114                         else
 115                                 ;//act OFF
 116                         break;
 117                 case BT9:
 118                         if(state) {
 119                                 ; //act ON
 120                         } else
 121                                 ;//act OFF
 122                         break;
 123                 case BT10:
 124                         if(state)
 125                                 ;//act ON
 126                         else
 127                                 ;//act OFF
 128                         break;
 129                 case BT11:
 130                         if(state)
 131                                 ; //act ON
 132                         else
 133                                 ;//act OFF
 134                         break;
 135                 case BT12:
 136                         if(state)
 137                                 ;//act ON
 138                         else
 139                                 ;//act OFF
 140                         break;
 141                 default:
 142                         printf("Unknown button pressed!\n");
 143                         break;
 144         }
 145 }
 146
 147 static void process_axis(int value, int id)
 148 {
 149         switch(id) {
 150                 case AXIS_X:
 151                 case AXIS_Y:
 152                         set_robot_speed();
 153                         break;
 154                 case AXIS_Z:
 155                         break;
 156                 case AXIS_R:
 157                         break;
 158                 case AXIS_S1:
 159                         switch (value) {
 160                         case 32767:
 161                                 act_jaws(CLOSE);
 162                                 printf("jaws CLOSE\n");
 163                                 break;
 164                         case -32767:
 165                                 act_jaws(OPEN);
 166                                 printf("jaws OPEN\n");
 167                                 break;
 168                         case 0:
 169                                 printf("jaws stop\n");
 170                                 break;
 171                         default:
 172                                 printf("error!\n");
 173                                 break;
 174                         }
 175                         break;
 176                 case AXIS_S2:
 177                         switch (value) {
 178                         case 32767:
 179                                 act_lift(DOWN, 0, 0);
 180                                 printf("lift DOWN\n");
 181                                 break;
 182                         case -32767:
 183                                 act_lift(UP, 0, 0);
 184                                 printf("lift UP\n");
 185                                 break;
 186                         case 0:
 187                                 printf("lift stop\n");
 188                                 break;
 189                         default:
 190                                 printf("error!\n");
 191                                 break;
 192                         }
 193                         break;
 194                 default:
 195                         printf("Unknown axis changed!\n");
 196                         break;
 197         }
 198 }
 199
 200 static void process_button(char state, int id)
 201 {
 202         if(state == 1) {
 203                 if(buttons_prev[id] != 1) {
 204                         state = ~state;
 205                         buttons_prev[id] = 1;
 206                 }
 207                 button_act(state, id);
 208         }
 209         else {
 210                 buttons_prev[id] = 0;
 211                 button_act(state, id);
 212         }
 213 }
 214
 215 static void int_handler(int sig)
 216 {
 217         printf("joyd stopping.\n");
 218
 219         /* orte domain destroy */
 220         robottype_roboorte_destroy(&orte);
 221
 222         /* close file descriptor */
 223         close(joy_fd);
 224
 225         exit(0);
 226 }
 227
 228 int open_joystick(char *name, int *num_of_axis, int *num_of_buttons)
 229 {
 230         int ret;
 231
 232         joy_fd = -1;
 233
 234         if((joy_fd=open("/dev/input/js0", O_NONBLOCK))==-1) {
 235                 printf("No joystick detected!\n");
 236                 goto ret;
 237         }
 238
 239         ioctl(joy_fd, JSIOCGNAME(80), name);
 240         ret = strcmp(JOY_NAME, name);
 241         if(ret != 0)
 242                 printf("Logitech joystick not present as js0\n");
 243         else {
 244                 ioctl(joy_fd, JSIOCGAXES, num_of_axis);
 245                 ioctl(joy_fd, JSIOCGBUTTONS, num_of_buttons);
 246                 goto ret;
 247         }
 248
 249         if((joy_fd=open("/dev/input/js1", O_NONBLOCK))==-1) {
 250                 printf("No joystick detected!\n");
 251                 goto ret;
 252         }
 253
 254         ioctl(joy_fd, JSIOCGNAME(80), name);
 255         ret = strcmp(JOY_NAME, name);
 256         if(ret != 0)
 257                 printf("Logitech joystick not present as js1\n");
 258         else {
 259                 ioctl(joy_fd, JSIOCGAXES, num_of_axis);
 260                 ioctl(joy_fd, JSIOCGBUTTONS, num_of_buttons);
 261                 goto ret;
 262         }
 263
 264 ret:
 265         return joy_fd;
 266 }
 267
 268 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
 269                         void *sendCallBackParam)
 270 {
 271 }
 272
 273 int main(int argc, char *argv[])
 274 {
 275         struct timespec timer;
 276         int num_of_axis=0, num_of_buttons=0;
 277         char joy_name[80];
 278         struct js_event js;
 279
 280         signal(SIGINT, int_handler);
 281
 282         orte.strength = 2;
 283
 284         /* orte initialization */
 285         if (robottype_roboorte_init(&orte)) {
 286                 printf("Unable to initialize ORTE.\n");
 287                 exit(1);
 288         }
 289
 290         act_init(&orte);
 291
 292         /* creating publishers */
 293         robottype_publisher_motion_speed_create(&orte, send_dummy_cb, &orte);
 294         robottype_publisher_lift_cmd_create(&orte, send_dummy_cb, &orte);
 295         robottype_publisher_jaws_cmd_create(&orte, send_dummy_cb, &orte);
 296
 297         if(open_joystick(joy_name, &num_of_axis, &num_of_buttons) == -1) {
 298                 printf("Joystick not found, exiting...\n");
 299                 return -1;
 300         }
 301
 302         printf("Joystick detected: %s\n\t%d axis\n\t%d buttons\n\n",
 303                                 joy_name, num_of_axis, num_of_buttons);
 304
 305         axis = (int *) calloc( num_of_axis, sizeof( int ) );
 306         buttons = (char *) calloc( num_of_buttons, sizeof( char ) );
 307         axis_prev = (int *) calloc( num_of_axis, sizeof( int ) );
 308         buttons_prev = (char *) calloc( num_of_buttons, sizeof( char ) );
 309
 310         while(1) {
 311                 timer.tv_sec = 0;
 312                 timer.tv_nsec = 100000000;
 313
 314                 while(read(joy_fd, &js, sizeof(struct js_event))>0) {
 315                         switch (js.type & ~JS_EVENT_INIT) {
 316                                 case JS_EVENT_AXIS:
 317                                         axis   [ js.number ] = js.value;
 318                                         process_axis(js.value, js.number);
 319                                         break;
 320                                 case JS_EVENT_BUTTON:
 321                                         process_button(js.value, js.number);
 322                                         buttons [ js.number ] = js.value;
 323                                         break;
 324                                 default:
 325                                         break;
 326                         }
 327
 328                 }
 329                 nanosleep(&timer,NULL);
 330         }
 331
 332         return 0;
 333 }