00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00025 #include <korebot/korebot.h>
00026 #include <linux/joystick.h>
00027 #include <signal.h>
00028
00029
00030 int sendflag = 0;
00031 int timeflag = 0;
00032 int actionflag = 0;
00033 int sock;
00034
00035 #define ACT_KOA 0
00036 #define ACT_CAM1 1
00037 #define ACT_CAM2 2
00038 #define ACT_BOTH 3
00039
00040 void handle_kill(int arg)
00041 {
00042 fprintf(stderr, "arg... killed\r\n");
00043 shutdown(sock,2);
00044
00045 close(sock);
00046
00047 exit(1);
00048 }
00049
00050 #define leftspeed 50
00051 #define rightspeed 50
00052 #define maxspeed 120
00053
00054 void handle_joyaxis(int number, int value, char * echoBuffer)
00055 {
00056 static int speed=0,right=rightspeed,left=leftspeed;
00057 static int campos0=0,campos1=0,campos2=0,campos3=0;
00058
00059 switch(actionflag)
00060 {
00061
00062 case ACT_KOA :
00063 if(number == 1) {
00064 if(right<30)
00065 right = 30;
00066 if(left<30)
00067 left = 30;
00068 speed = -((value*maxspeed)/32767);
00069 sprintf(echoBuffer,"setspeed %d %d\n",(speed*right)/100,(speed*left)/100);
00070 sendflag = 1;
00071 timeflag = 1;
00072 }
00073
00074 if(number == 0) {
00075 right = (((value+32767)*100)/65534);
00076 left = 100 - right;
00077 if(abs(speed) < 5) {
00078 sprintf(echoBuffer,"setspeed %d %d\n",right-rightspeed,left-leftspeed);
00079 }
00080 else {
00081 if(right<30)
00082 right = 30;
00083 if(left<30)
00084 left = 30;
00085 sprintf(echoBuffer,"setspeed %d %d\n",(speed*right)/100,(speed*left)/100);
00086 }
00087 sendflag = 1;
00088 timeflag = 1;
00089 }
00090 break;
00091
00092
00093 case ACT_CAM1 :
00094 if(number == 1) {
00095 campos0 = ((value*50)/32767) + 50;
00096
00097 ksock_send_command(sock,"movecamera %d %d",campos1, campos0);
00098
00099 timeflag = 0;
00100 }
00101
00102 if(number == 0) {
00103 campos1 = 50 - ((value*50)/32767);
00104
00105 ksock_send_command(sock,"movecamera %d %d",campos1, campos0);
00106
00107 timeflag = 0;
00108 }
00109 break;
00110
00111
00112 case ACT_CAM2 :
00113 if(number == 1) {
00114 campos0 = ((value*50)/32767) + 50;
00115
00116 ksock_send_command(sock,"movecamera2 %d %d",campos1, campos0);
00117
00118 timeflag = 0;
00119 }
00120
00121 if(number == 0) {
00122 campos1 = 50 - ((value*50)/32767);
00123
00124 ksock_send_command(sock,"movecamera2 %d %d",campos1, campos0);
00125
00126 timeflag = 0;
00127 }
00128 break;
00129
00130
00131 case ACT_BOTH :
00132 if(number == 1) {
00133 campos0 = ((value*50)/32767) + 50;
00134 sprintf(echoBuffer,"moveboth %d %d\n",campos1,campos0);
00135 sendflag = 1;
00136 timeflag = 0;
00137 }
00138
00139 if(number == 0) {
00140 campos1 = 50 - ((value*50)/32767);
00141 sprintf(echoBuffer,"moveboth %d %d\n",campos1,campos0);
00142 sendflag = 1;
00143 timeflag = 0;
00144 }
00145 break;
00146 }
00147 }
00148
00149 void handle_joybutton(int number, int value, char * echoBuffer)
00150 {
00151 switch(number)
00152 {
00153 case 0 :
00154
00155 if(value)
00156 ksock_send_command(sock,"playsound %d",1);
00157 break;
00158 case 1 :
00159
00160 if(value)
00161 ksock_send_command(sock,"playsound %d",4);
00162 break;
00163 case 2 :
00164 if(value)
00165 actionflag = ACT_BOTH;
00166 else
00167 if(actionflag == ACT_BOTH)
00168 {
00169
00170
00171 actionflag = ACT_KOA;
00172 }
00173 break;
00174 case 3 :
00175 if(value)
00176 actionflag = ACT_CAM2;
00177 else
00178 if(actionflag == ACT_CAM2)
00179 {
00180 sprintf(echoBuffer,"movecamera2 %d %d\n",50,50);
00181 sendflag = 1;
00182 timeflag = 1;
00183 actionflag = ACT_KOA;
00184 }
00185 break;
00186 case 4 :
00187 if(value)
00188 actionflag = ACT_CAM1;
00189 else
00190 if(actionflag == ACT_CAM1)
00191 {
00192 sprintf(echoBuffer,"movecamera %d %d\n",50,50);
00193 sendflag = 1;
00194 timeflag = 1;
00195 actionflag = ACT_KOA;
00196 }
00197 break;
00198 case 5 :
00199
00200 if(value)
00201 ksock_send_command(sock,"playsound %d",2);
00202 break;
00203 case 6 :
00204
00205 if(value)
00206 ksock_send_command(sock,"playsound %d",3);
00207 break;
00208 case 10 :
00209
00210 if(value)
00211 ksock_send_command(sock,"initcamera");
00212 break;
00213 }
00214 }
00215
00216 #define RCVBUFSIZE 32
00217
00218 int main(int argc, char *argv[])
00219 {
00220 struct timeval lastcommand, now, diff;
00221 struct timezone here;
00222
00223 char *servIP;
00224 unsigned short servPort;
00225 char echoBuffer[RCVBUFSIZE];
00226 unsigned int echoStringLen;
00227 int bytesRcvd, totalBytesRcvd;
00228
00229 int fd;
00230 int readevent;
00231 struct js_event e;
00232
00233
00234 signal(SIGINT, handle_kill);
00235
00236
00237 gettimeofday(&lastcommand,&here);
00238
00239 if (argc < 3)
00240 {
00241 fprintf(stderr, "Usage: %s <Server IP> <Port>\n", argv[0]);
00242 exit(1);
00243 }
00244
00245
00246 fd = open ("/dev/input/js0", O_RDONLY|O_NONBLOCK);
00247 if(fd < 0)
00248 {
00249 printf("Cannot open joystick(/dev/input/js0)\r\n");
00250 exit(1);
00251 }
00252
00253 servIP = argv[1];
00254
00255 servPort = atoi(argv[2]);
00256
00257 ksock_init('\n',2048);
00258
00259 sock = ksock_connect(servIP, servPort);
00260 if(sock > 0)
00261 printf("Connected to %s:%d\r\n",servIP,servPort);
00262
00263
00264 readevent = read (fd, &e, sizeof(struct js_event));
00265
00266 while (readevent >=0 || sendflag || errno == EAGAIN)
00267 {
00268 if(readevent >= 0)
00269 {
00270
00271 e.type &= ~JS_EVENT_INIT;
00272 switch (e.type)
00273 {
00274
00275
00276 case JS_EVENT_BUTTON:
00277
00278 handle_joybutton(e.number,e.value,echoBuffer);
00279 break;
00280
00281
00282 case JS_EVENT_AXIS:
00283
00284 handle_joyaxis(e.number,e.value,echoBuffer);
00285 break;
00286 }
00287 }
00288
00289
00290 if(sendflag) {
00291
00292 gettimeofday(&now,&here);
00293 timersub(&now,&lastcommand,&diff);
00294
00295
00296 if(diff.tv_usec > 100000 || !timeflag)
00297 {
00298 lastcommand.tv_sec = now.tv_sec;
00299 lastcommand.tv_usec = now.tv_usec;
00300
00301 printf("%s\n",echoBuffer);
00302
00303 echoStringLen = strlen(echoBuffer);
00304 if (send(sock, echoBuffer, echoStringLen, 0) != echoStringLen)
00305 DieWithError("send() sent a different number of bytes than expected");
00306
00307 totalBytesRcvd = 0;
00308 printf("Received: ");
00309 while (totalBytesRcvd < echoStringLen)
00310 {
00311
00312
00313 if ((bytesRcvd = recv(sock, echoBuffer, RCVBUFSIZE - 1, 0)) <= 0)
00314 DieWithError("recv() failed or connection closed prematurely");
00315 totalBytesRcvd += bytesRcvd;
00316 echoBuffer[bytesRcvd] = '\0';
00317 printf(echoBuffer);
00318 }
00319
00320 printf("\n");
00321
00322 sendflag = 0;
00323 }
00324 }
00325
00326
00327 readevent = read (fd, &e, sizeof(struct js_event));
00328 }
00329
00330 close(fd);
00331 close(sock);
00332 exit(0);
00333 }