diff --git a/bin/Debug/UDPclient b/bin/Debug/UDPclient index 6e3527b..649fc1b 100755 Binary files a/bin/Debug/UDPclient and b/bin/Debug/UDPclient differ diff --git a/client.config b/client.config index bc98dfa..844644d 100644 --- a/client.config +++ b/client.config @@ -1,6 +1,6 @@ KEYBOARD /dev/input/event2 JOYSTICK /dev/input/js0 FRAME_US 100000 -SERVER 10.8.212.200:32000 +SERVER 10.8.245.47:32000 TIMEOUT 250 CLINBR 0 diff --git a/main.c b/main.c index be2d3c0..6f2576a 100644 --- a/main.c +++ b/main.c @@ -86,6 +86,8 @@ void *keyboard_thread_fcn(void * arg) args->frame = data2frame(event2data(&args->hl, ev)); args->outtty.motor_r = (2*0b01111 - args->hl.velocity - args->hl.direction)*40; args->outtty.motor_l = (- args->hl.velocity + args->hl.direction)*40; + + /* args->outtty.servo_pan += args->hl.pan_left - args->hl.pan_right; if(args->outtty.servo_pan>50) args->outtty.servo_pan = 50; if(args->outtty.servo_pan<-50) args->outtty.servo_pan = -50; @@ -96,6 +98,9 @@ void *keyboard_thread_fcn(void * arg) args->outtty.servo_pan = 0; args->outtty.servo_tilt= 0; } + */ + + } // quit if(16==ev.code && 1==ev.value) break; @@ -148,7 +153,7 @@ void *joystick_thread_fcn(void * arg) } } if(JS_EVENT_AXIS == ev.type){ - //printf("%d %d %d\n", ev.type, ev.value, ev.number); + printf("%d %d %d\n", ev.type, ev.value, ev.number); if(0==ev.number){ jsdata.x = ev.value; } @@ -158,6 +163,38 @@ void *joystick_thread_fcn(void * arg) jsdata.y = +32767; } } + if(2==ev.number){ + outttyloc.servo_tilt = 0; + if(-30000 > ev.value){ + outttyloc.servo_tilt = -1; + args->hl.tilt_down = 1; + args->hl.tilt_up = 0; + }else{ + args->hl.tilt_down = 0; + if(+5000>ev.value){ + args->hl.tilt_up = 1; + outttyloc.servo_tilt = +1; + }else{ + args->hl.tilt_up = 0; + } + } + } + if(3==ev.number){ + outttyloc.servo_pan = 0; + if(-30000 > ev.value){ + args->hl.pan_left = 1; + args->hl.pan_right = 0; + outttyloc.servo_pan = -1; + }else{ + args->hl.pan_left = 0; + if(+5000>ev.value){ + args->hl.pan_right = 1; + outttyloc.servo_pan = +1; + }else{ + args->hl.pan_right = 0; + } + } + } } outttyloc.motor_r = jsdata.y/30 - jsdata.x/30; outttyloc.motor_l = jsdata.y/30 + jsdata.x/30 ; @@ -167,6 +204,7 @@ void *joystick_thread_fcn(void * arg) if(-1022>outttyloc.motor_l) outttyloc.motor_l = -1023; args->outtty = outttyloc; + args->hl.ignation = (1 & (jsdata.buttons >> 2)); printf("%6d %6d %4x\n", jsdata.x, jsdata.y, jsdata.buttons); } @@ -338,6 +376,8 @@ int main(int argc, char* argv[]) memset(refl_thread_args.timestamps, 0, 256*sizeof(uint64_t)); frame_nbr = 0; + senddata.outtty.servo_pan = 0; + senddata.outtty.servo_tilt = 0; while(1){ usleep(conf.frame_us); @@ -358,14 +398,25 @@ int main(int argc, char* argv[]) senddata.outtty.motor_l = keyboard_thread_args.outtty.motor_l + joystick_thread_args.outtty.motor_l; senddata.outtty.motor_r = keyboard_thread_args.outtty.motor_r + joystick_thread_args.outtty.motor_r; - senddata.outtty.servo_pan = keyboard_thread_args.outtty.servo_pan + joystick_thread_args.outtty.servo_pan; - senddata.outtty.servo_tilt = keyboard_thread_args.outtty.servo_tilt + joystick_thread_args.outtty.servo_tilt; + senddata.outtty.servo_pan += keyboard_thread_args.hl.pan_right - keyboard_thread_args.hl.pan_left + joystick_thread_args.hl.pan_right - joystick_thread_args.hl.pan_left; + senddata.outtty.servo_tilt += keyboard_thread_args.hl.tilt_up - keyboard_thread_args.hl.tilt_down + joystick_thread_args.hl.tilt_up - joystick_thread_args.hl.tilt_down; + + if(keyboard_thread_args.hl.ignation | joystick_thread_args.hl.ignation){ + senddata.outtty.servo_pan = 0; + senddata.outtty.servo_tilt = 0; + } if(1022senddata.outtty.motor_l) senddata.outtty.motor_l = -1023; if(1022senddata.outtty.motor_r) senddata.outtty.motor_r = -1023; + if(senddata.outtty.servo_pan>50) senddata.outtty.servo_pan = 50; + if(senddata.outtty.servo_pan<-50) senddata.outtty.servo_pan = -50; + if(senddata.outtty.servo_tilt>50) senddata.outtty.servo_tilt = 50; + if(senddata.outtty.servo_tilt<-50) senddata.outtty.servo_tilt = -50; + + n_send = sendto(sockfd, &senddata, sizeof(senddata), 0, (struct sockaddr *)&servaddr, sizeof(servaddr)); printf("SEND FRAME -- FRM_NBR: %5d, BYTES send: %3d, SEND_FRM: %#x, CLINBR: %d, CLISEL: %d, SERVOFF: %d\n", frame_nbr, n_send, frame, senddata.clinbr, senddata.clisel, senddata.servoff); diff --git a/obj/Debug/main.o b/obj/Debug/main.o index 5cb1cab..3ca2d68 100644 Binary files a/obj/Debug/main.o and b/obj/Debug/main.o differ