ChCh
/
HengLongUDPclient
mirror of https://github.com/TheTesla/HengLongUDPclient.git
5
0
Fork 0
Code Issues Releases Wiki Activity

added joystick support

This commit is contained in:
chch 2014-02-16 04:36:01 +01:00
parent 3b507ff03c
commit 85e30d3d43
8 changed files with 173 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
UDPclient.cbp
View File

@ -29,6 +29,7 @@
</Target>
</Build>
<Compiler>
<Add option="-O1" />
<Add option="-Wall" />
</Compiler>
<Linker>

31
UDPclient.depend
View File

@ -29,3 +29,34 @@
<stdio.h>
<inttypes.h>
1392499886 source:/home/chch/HengLongUDPclient/main.c
<pthread.h>
<stdio.h>
<stdlib.h>
<string.h>
<unistd.h>
<errno.h>
<fcntl.h>
<dirent.h>
<linux/input.h>
<sys/types.h>
<sys/stat.h>
<sys/select.h>
<sys/time.h>
<termios.h>
<signal.h>
<sys/socket.h>
<netinet/in.h>
<arpa/inet.h>
<inttypes.h>
"henglong.h"
<linux/joystick.h>
1392494240 /home/chch/HengLongUDPclient/henglong.h
<linux/input.h>
<stdio.h>
<inttypes.h>
1392494235 source:/home/chch/HengLongUDPclient/henglong.c
"henglong.h"

24
UDPclient.layout
View File

@ -1,24 +1,16 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_layout_file>
<ActiveTarget name="Debug" />
<File name="henglong.h" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="303" topLine="0" />
</Cursor>
<File name="client.config" open="1" top="0" tabpos="4">
<Cursor position="114" topLine="0" />
</File>
<File name="client.config" open="1" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="62" topLine="0" />
</Cursor>
<File name="henglong.c" open="1" top="0" tabpos="2">
<Cursor position="804" topLine="0" />
</File>
<File name="henglong.c" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="3563" topLine="94" />
</Cursor>
<File name="henglong.h" open="1" top="0" tabpos="3">
<Cursor position="564" topLine="0" />
</File>
<File name="main.c" open="1" top="1" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="2309" topLine="99" />
</Cursor>
<File name="main.c" open="1" top="1" tabpos="1">
<Cursor position="7317" topLine="179" />
</File>
</CodeBlocks_layout_file>

BIN
bin/Debug/UDPclient
View File

Binary file not shown.

5
client.config
View File

@ -1,5 +1,6 @@
INPUTDEV /dev/input/event2
KEYBOARD /dev/input/event2
JOYSTICK /dev/input/js0
FRAME_US 100000
SERVER 127.0.0.1:32000
SERVER 10.8.212.200:32000
TIMEOUT 250
CLINBR 0

157
main.c
View File

@ -20,6 +20,7 @@
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include "henglong.h"
#include <linux/joystick.h>
typedef struct outtty_t
{
@ -37,33 +38,41 @@ typedef struct input_thread_t
outtty_t outtty;
} input_thread_t;
void initouttty(outtty_t* tty)
{
tty->motor_l = 0;
tty->motor_r = 0;
tty->servo_pan = 0;
tty->servo_tilt = 0;
}
typedef struct RCdatagram_t
{
uint16_t frame_nbr;
uint64_t time_us;
int frame_recv;
int32_t frame_recv;
uint8_t clisel;
uint8_t clinbr;
uint8_t client_selected;
unsigned char servoff;
uint8_t servoff;
outtty_t outtty;
} RCdatagram_t;
} __attribute__ ((packed)) RCdatagram_t;
void *input_thread_fcn(void * arg)
void *keyboard_thread_fcn(void * arg)
{
printf("pthread input started\n");
struct input_event ev;
int fevdev;
int size = sizeof(struct input_event);
int size = sizeof(ev);
int rd;
input_thread_t* args;
args = (input_thread_t*) arg;
fevdev = open(args->filename, O_RDONLY);
if (fevdev == -1) {
printf("Failed to open event device.\n");
exit(1);
printf("Failed to open keyboard device.\n");
pthread_exit(0);
}
while (1)
@ -92,7 +101,7 @@ void *input_thread_fcn(void * arg)
if(16==ev.code && 1==ev.value) break;
}
printf("Exiting input thread.\n");
printf("Exiting keyboard thread.\n");
ioctl(fevdev, EVIOCGRAB, 1);
close(fevdev);
@ -100,6 +109,75 @@ void *input_thread_fcn(void * arg)
}
void *joystick_thread_fcn(void * arg)
{
printf("pthread input started\n");
struct js_event ev;
struct JS_DATA_TYPE jsdata;
int fevdev;
int size = sizeof(ev);
int rd;
outtty_t outttyloc;
input_thread_t* args;
args = (input_thread_t*) arg;
fevdev = open(args->filename, O_RDONLY);
if (fevdev == -1) {
printf("Failed to open joystick device.\n");
pthread_exit(0);
}
jsdata.x = 0;
jsdata.y = 0;
jsdata.buttons = 0;
while (1)
{
if ((rd = read(fevdev, &ev, size)) < size) {
break;
}
if(JS_EVENT_BUTTON == ev.type) {
//printf("%d %d %d\n", ev.type, ev.value, ev.number);
if(1==ev.value){
jsdata.buttons |= 1<<ev.number;
}
if(0==ev.value){
jsdata.buttons &= ~(1<<ev.number);
}
}
if(JS_EVENT_AXIS == ev.type){
//printf("%d %d %d\n", ev.type, ev.value, ev.number);
if(0==ev.number){
jsdata.x = ev.value;
}
if(1==ev.number){
jsdata.y = -ev.value;
if(-32768==ev.value){
jsdata.y = +32767;
}
}
}
outttyloc.motor_r = jsdata.y/30 - jsdata.x/30;
outttyloc.motor_l = jsdata.y/30 + jsdata.x/30 ;
if(1022<outttyloc.motor_r) outttyloc.motor_r = 1023;
if(-1022>outttyloc.motor_r) outttyloc.motor_r = -1023;
if(1022<outttyloc.motor_l) outttyloc.motor_l = 1023;
if(-1022>outttyloc.motor_l) outttyloc.motor_l = -1023;
args->outtty = outttyloc;
printf("%6d %6d %4x\n", jsdata.x, jsdata.y, jsdata.buttons);
}
printf("Exiting joystick thread.\n");
close(fevdev);
pthread_exit(0);
}
typedef struct refl_thread_args_t
{
int sockfd;
@ -133,7 +211,7 @@ void *refl_thread_fcn(void* arg)
refl_thread_args_ptr = (refl_thread_args_t*) arg;
while(1){
n=recvfrom(refl_thread_args_ptr->sockfd,&refldata,64,0,NULL,NULL);
n=recvfrom(refl_thread_args_ptr->sockfd,&refldata,sizeof(refldata),0,NULL,NULL);
frame_nbr_refl = refldata.frame_nbr;
@ -155,7 +233,8 @@ void *refl_thread_fcn(void* arg)
typedef struct henglongconf_t
{
uint32_t frame_us;
char inpdevname[256];
char keyboarddevname[256];
char joystickdevname[256];
in_addr_t ip; // v4 only
uint16_t port;
uint8_t timeout;
@ -174,14 +253,18 @@ henglongconf_t getconfig(char* conffilename)
// defaults
conf.timeout = 16;
conf.frame_us = 100000;
strcpy(conf.inpdevname, "/dev/input/event2");
conf.port = 32000;
conf.ip = inet_addr("127.0.0.1");
conf.keyboarddevname[0] = 0;
conf.joystickdevname[0] = 0;
while(fgets(line, 256, configFile)){
sscanf(line, "%16s %256s", parameter, value);
if(0==strcmp(parameter,"INPUTDEV")){
sscanf(value, "%256s", conf.inpdevname);
if(0==strcmp(parameter,"KEYBOARD")){
sscanf(value, "%256s", conf.keyboarddevname);
}
if(0==strcmp(parameter,"JOYSTICK")){
sscanf(value, "%256s", conf.joystickdevname);
}
if(0==strcmp(parameter,"FRAME_US")){
sscanf(value, "%" SCNu32 , &conf.frame_us);
@ -203,8 +286,9 @@ henglongconf_t getconfig(char* conffilename)
int main(int argc, char* argv[])
{
pthread_t inpthread, refl_thread;
input_thread_t input_thread_args;
pthread_t keybthread, joythread, refl_thread;
input_thread_t keyboard_thread_args;
input_thread_t joystick_thread_args;
refl_thread_args_t refl_thread_args;
int frame = 0;
uint16_t frame_nbr;
@ -214,21 +298,31 @@ int main(int argc, char* argv[])
henglongconf_t conf;
RCdatagram_t senddata;
if(2!=argc){
printf("\nThis program is intented to be run on the PC as client to control the server on the heng long tank. \n\n USAGE: UDPclient client.config\n\n Copyright (C) 2014 Stefan Helmert <stefan.helmert@gmx.net>\n\n");
return 0;
}
inithenglong(&input_thread_args.hl);
inithenglong(&keyboard_thread_args.hl);
initouttty(&keyboard_thread_args.outtty);
inithenglong(&joystick_thread_args.hl);
initouttty(&joystick_thread_args.outtty);
conf = getconfig(argv[1]);
input_thread_args.filename = conf.inpdevname;
if (pthread_create(&inpthread, NULL, input_thread_fcn , (void *) &input_thread_args)) printf("failed to create thread\n");
joystick_thread_args.filename = conf.joystickdevname;
keyboard_thread_args.filename = conf.keyboarddevname;
if(keyboard_thread_args.filename[0]){
if (pthread_create(&keybthread, NULL, keyboard_thread_fcn , (void *) &keyboard_thread_args)) printf("failed to create keyboard thread\n");
}else{
printf("no keyboard!\n");
}
if(joystick_thread_args.filename[0]){
if (pthread_create(&joythread, NULL, joystick_thread_fcn , (void *) &joystick_thread_args)) printf("failed to create joystick thread\n");
}else{
printf("no joystick!\n");
}
sockfd = socket(AF_INET,SOCK_DGRAM,0);
@ -247,7 +341,7 @@ int main(int argc, char* argv[])
while(1){
usleep(conf.frame_us);
frame = input_thread_args.frame;
frame = 0xc0ffee;
time_us = get_us();
frame_nbr++;
@ -259,14 +353,23 @@ int main(int argc, char* argv[])
senddata.time_us = time_us;
senddata.frame_recv = frame;
senddata.clinbr = conf.clinbr;
senddata.clisel = input_thread_args.hl.clisel;
senddata.servoff = input_thread_args.hl.servoff;
senddata.outtty = input_thread_args.outtty;
senddata.clisel = keyboard_thread_args.hl.clisel;
senddata.servoff = keyboard_thread_args.hl.servoff;
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;
if(1022<senddata.outtty.motor_l) senddata.outtty.motor_l = 1023;
if(-1022>senddata.outtty.motor_l) senddata.outtty.motor_l = -1023;
if(1022<senddata.outtty.motor_r) senddata.outtty.motor_r = 1023;
if(-1022>senddata.outtty.motor_r) senddata.outtty.motor_r = -1023;
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, conf.clinbr, input_thread_args.hl.clisel, input_thread_args.hl.servoff);
if(pthread_kill(inpthread, 0)) break;
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);
if(pthread_kill(keybthread, 0)) break;
}
return 0;

BIN
obj/Debug/henglong.o
View File

Binary file not shown.

BIN
obj/Debug/main.o
View File

Binary file not shown.