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 void handle_joyaxis(int number, int value, char * echoBuffer)
00051 {
00052 static int campos0=0,campos1=0,campos2=0,campos3=0;
00053
00054 switch(actionflag)
00055 {
00056
00057 case ACT_CAM1 :
00058 if(number == 1) {
00059 campos0 = ((value*50)/32767) + 50;
00060
00061 ksock_send_command(sock,"movecamera %d %d",campos1, campos0);
00062
00063 timeflag = 0;
00064 }
00065
00066 if(number == 0) {
00067 campos1 = 50 - ((value*50)/32767);
00068
00069 ksock_send_command(sock,"movecamera %d %d",campos1, campos0);
00070
00071 timeflag = 0;
00072 }
00073 break;
00074
00075
00076 case ACT_CAM2 :
00077 if(number == 1) {
00078 campos0 = ((value*50)/32767) + 50;
00079
00080 ksock_send_command(sock,"movecamera2 %d %d",campos1, campos0);
00081
00082 timeflag = 0;
00083 }
00084
00085 if(number == 0) {
00086 campos1 = 50 - ((value*50)/32767);
00087
00088 ksock_send_command(sock,"movecamera2 %d %d",campos1, campos0);
00089
00090 timeflag = 0;
00091 }
00092 break;
00093
00094
00095 case ACT_BOTH :
00096 if(number == 1) {
00097 campos0 = ((value*50)/32767) + 50;
00098 sprintf(echoBuffer,"moveboth %d %d\n",campos1,campos0);
00099 sendflag = 1;
00100 timeflag = 0;
00101 }
00102
00103 if(number == 0) {
00104 campos1 = 50 - ((value*50)/32767);
00105 sprintf(echoBuffer,"moveboth %d %d\n",campos1,campos0);
00106 sendflag = 1;
00107 timeflag = 0;
00108 }
00109 break;
00110 }
00111 }
00112
00113 void handle_joybutton(int number, int value, char * echoBuffer)
00114 {
00115 switch(number)
00116 {
00117 case 2 :
00118 if(value)
00119 actionflag = ACT_BOTH;
00120 else
00121 if(actionflag == ACT_BOTH)
00122 {
00123
00124
00125 actionflag = ACT_KOA;
00126 }
00127 break;
00128 case 3 :
00129 if(value)
00130 actionflag = ACT_CAM2;
00131 else
00132 if(actionflag == ACT_CAM2)
00133 {
00134 sprintf(echoBuffer,"movecamera2 %d %d\n",50,50);
00135 sendflag = 1;
00136 timeflag = 1;
00137 actionflag = ACT_KOA;
00138 }
00139 break;
00140 case 4 :
00141 if(value)
00142 actionflag = ACT_CAM1;
00143 else
00144 if(actionflag == ACT_CAM1)
00145 {
00146 sprintf(echoBuffer,"movecamera %d %d\n",50,50);
00147 sendflag = 1;
00148 timeflag = 1;
00149 actionflag = ACT_KOA;
00150 }
00151 break;
00152 case 10 :
00153
00154 if(value)
00155 ksock_send_command(sock,"initcamera");
00156 break;
00157 }
00158 }
00159
00160 #define RCVBUFSIZE 32
00161
00162 int main(int argc, char *argv[])
00163 {
00164 struct timeval lastcommand, now, diff;
00165 struct timezone here;
00166
00167 char *servIP;
00168 unsigned short servPort;
00169 char echoBuffer[RCVBUFSIZE];
00170 unsigned int echoStringLen;
00171 int bytesRcvd, totalBytesRcvd;
00172
00173 int fd;
00174 int readevent;
00175 struct js_event e;
00176
00177
00178 signal(SIGINT, handle_kill);
00179
00180
00181 gettimeofday(&lastcommand,&here);
00182
00183 if (argc < 3)
00184 {
00185 fprintf(stderr, "Usage: %s <Server IP> <Port>\n", argv[0]);
00186 exit(1);
00187 }
00188
00189
00190 fd = open ("/dev/input/js0", O_RDONLY|O_NONBLOCK);
00191 if(fd < 0)
00192 {
00193 printf("Cannot open joystick(/dev/input/js0)\r\n");
00194 exit(1);
00195 }
00196
00197 servIP = argv[1];
00198
00199 servPort = atoi(argv[2]);
00200
00201 ksock_init('\n',2048);
00202
00203 sock = ksock_connect(servIP, servPort);
00204 if(sock > 0)
00205 printf("Connected to %s:%d\r\n",servIP,servPort);
00206
00207
00208 readevent = read (fd, &e, sizeof(struct js_event));
00209
00210 while (readevent >=0 || sendflag || errno == EAGAIN)
00211 {
00212 if(readevent >= 0)
00213 {
00214
00215 e.type &= ~JS_EVENT_INIT;
00216 switch (e.type)
00217 {
00218
00219
00220 case JS_EVENT_BUTTON:
00221
00222 handle_joybutton(e.number,e.value,echoBuffer);
00223 break;
00224
00225
00226 case JS_EVENT_AXIS:
00227
00228 handle_joyaxis(e.number,e.value,echoBuffer);
00229 break;
00230 }
00231 }
00232
00233
00234 if(sendflag) {
00235
00236 gettimeofday(&now,&here);
00237 timersub(&now,&lastcommand,&diff);
00238
00239
00240 if(diff.tv_usec > 100000 || !timeflag)
00241 {
00242 lastcommand.tv_sec = now.tv_sec;
00243 lastcommand.tv_usec = now.tv_usec;
00244
00245 printf("%s\n",echoBuffer);
00246
00247 echoStringLen = strlen(echoBuffer);
00248 if (send(sock, echoBuffer, echoStringLen, 0) != echoStringLen)
00249 DieWithError("send() sent a different number of bytes than expected");
00250
00251 totalBytesRcvd = 0;
00252 printf("Received: ");
00253 while (totalBytesRcvd < echoStringLen)
00254 {
00255
00256
00257 if ((bytesRcvd = recv(sock, echoBuffer, RCVBUFSIZE - 1, 0)) <= 0)
00258 DieWithError("recv() failed or connection closed prematurely");
00259 totalBytesRcvd += bytesRcvd;
00260 echoBuffer[bytesRcvd] = '\0';
00261 printf(echoBuffer);
00262 }
00263
00264 printf("\n");
00265
00266 sendflag = 0;
00267 }
00268 }
00269
00270
00271 readevent = read (fd, &e, sizeof(struct js_event));
00272 }
00273
00274 close(fd);
00275 close(sock);
00276 exit(0);
00277 }