hexapod_ng/legacy/linux/js_udp_control/jscontrol.c

#include <stdio.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <linux/joystick.h>

#include <netinet/in.h>

#include "licks_message.h"

int joystick_fd = -1;
struct js_event event;

char *jsdev="/dev/input/js0";

int udp_socket_fd;

struct sockaddr_in localaddr;
struct sockaddr_in remoteaddr;

licks_net_message m;


#define MODE_IDLE      0
#define MODE_TRANSLATE 1
#define MODE_ROTATE    2 
#define MODE_WALK      3

int
main(int argc, char **argv) {
   int quit=0;
   int power=0;
   int joy_x,joy_y,joy_z,joy_rz;
   
   int mode;
   
   if((joystick_fd=open(jsdev,O_RDONLY|O_NONBLOCK))<0) {
      perror("open");
      exit(0);
   }

   udp_socket_fd=socket(PF_INET,SOCK_DGRAM,0);
   if(udp_socket_fd==-1) {
      perror("socket");
      exit(0);
   }

   localaddr.sin_family=AF_INET;
   localaddr.sin_port=htons(1337);
   inet_aton("0.0.0.0",&localaddr.sin_addr);

   remoteaddr.sin_family=AF_INET;
   remoteaddr.sin_port=htons(1337);
   inet_aton("192.168.1.43",&remoteaddr.sin_addr);


   if(bind(udp_socket_fd,(struct sockaddr *)&localaddr,sizeof(struct sockaddr_in))) {
      perror("bind");
      exit(0);
   }


   
   joy_x=0; joy_y=0; joy_z=0; joy_rz=0;

   mode=MODE_IDLE;

   while(!quit) {
      while(read(joystick_fd,&event,sizeof(struct js_event))==sizeof(struct js_event)) {
          switch(event.type) {
             case JS_EVENT_AXIS:
                switch(event.number) {
                   case 0:
                     joy_x=event.value;
                     break;
                   case 1:
                     joy_y=event.value;
                     break;
                   case 2:
                     joy_z=event.value;
                     break;
                   case 3:
                     joy_rz=event.value;
                     break;
                }
                break;
             case JS_EVENT_BUTTON:
               printf("button %d value %d\n",event.number,event.value);
               if(event.value) {
                  switch(event.number) {
                     case 6:
                        mode=MODE_ROTATE;
                        break;
                     case 7:
                        mode=MODE_TRANSLATE;
                        break;
                     case 9:
                        power=1-power;
                        m.type=MSG_POWER;
                        m.i=power;
                        sendto(udp_socket_fd,&m,sizeof(licks_net_message),0,(struct sockaddr *)&remoteaddr,sizeof(struct sockaddr_in));
                        break;                     
                     default:
                        mode=MODE_WALK;
                  }
               } else {
                  mode=MODE_WALK;
               }
               break;
          }
      }
      switch(mode) {
         case MODE_TRANSLATE:
            m.type=MSG_MOVE_BODY;
            if(abs(joy_y)>5000) {
               m.move_parameters.v.x=-joy_y/100;
            } else {
               m.move_parameters.v.x=0;
            }
            if(abs(joy_x)>5000) {
               m.move_parameters.v.y=-joy_x/100;
            } else {
               m.move_parameters.v.y=0;
            }
            if(abs(joy_z)>5000) {
               m.move_parameters.v.z=joy_z/100;
            } else {
               m.move_parameters.v.z=0;
            }
            m.move_parameters.duration=1;
            sendto(udp_socket_fd,&m,sizeof(licks_net_message),0,(struct sockaddr *)&remoteaddr,sizeof(struct sockaddr_in));
            break;
            
         case MODE_ROTATE:
            m.type=MSG_ROTATE_BODY;
            if(abs(joy_x)>5000) {
               m.move_parameters.v.x=-joy_x/100;
            } else {
               m.move_parameters.v.x=0;
            }
            if(abs(joy_y)>5000) {
               m.move_parameters.v.y=joy_y/100;
            } else {
               m.move_parameters.v.y=0;
            }
            if(abs(joy_rz)>5000) {
               m.move_parameters.v.z=-joy_rz/100;
            } else {
               m.move_parameters.v.z=0;
            }
            m.move_parameters.duration=1;
            sendto(udp_socket_fd,&m,sizeof(licks_net_message),0,(struct sockaddr *)&remoteaddr,sizeof(struct sockaddr_in));
            break;
         case MODE_WALK:
            m.type=MSG_WALK;
            if(abs(joy_y)>5000) {
               m.move_parameters.v.x=-joy_y/2;
            } else {
               m.move_parameters.v.x=0;
            }
            if(abs(joy_x)>5000) {
               m.move_parameters.v.y=-joy_x/2;
            } else {
               m.move_parameters.v.y=0;
            }
            if(abs(joy_rz)>5000) {
               m.move_parameters.v.z=joy_rz/327.6;
            } else {
               m.move_parameters.v.z=0;
            }
            sendto(udp_socket_fd,&m,sizeof(licks_net_message),0,(struct sockaddr *)&remoteaddr,sizeof(struct sockaddr_in));
            break;
        

     }
     usleep(10000);
   }


   close(udp_socket_fd);
   close(joystick_fd);
}