00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00025 #include <korebot/korebot.h>
00026
00027 #define RCVBUFSIZE 32
00028
00029 knet_dev_t *koala;
00030 int right = 0;
00031 int left = 0;
00032 int left_limit_p = 100;
00033 int left_limit_n = -100;
00034 int right_limit_p = 100;
00035 int right_limit_n = -100;
00036
00037 knet_dev_t *motor0, *motor1, *motor2, *motor3;
00038 int32_t minpos0,maxpos0,minpos1,maxpos1;
00039 int32_t minpos2,maxpos2,minpos3,maxpos3;
00040
00041
00042 const char * soundlist[] = {
00043 "sound1.wav",
00044 "sound2.wav",
00045 "sound3.wav",
00046 "sound4.wav",
00047 "sound5.wav"
00048 };
00049 const unsigned int soundlist_size = sizeof(soundlist)/sizeof(char*);
00050
00051
00052 typedef struct koasound_s {
00053 int sound;
00054 struct koasound_s * next;
00055 }koasound_t;
00056
00057 koasound_t * lastsound = NULL;
00058 koasound_t * currentsound = NULL;
00059 pthread_mutex_t soundlock;
00060
00061
00065 void * koa_init_task(void * arg)
00066 {
00067 kmot_ResetError(motor0);
00068 kmot_ResetError(motor1);
00069 kmot_ResetError(motor2);
00070 kmot_ResetError(motor3);
00071 kmot_SearchLimits(motor0, 1, 3, &minpos0, &maxpos0,100000);
00072 printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0);
00073 kmot_SearchLimits(motor1, 5, 3, &minpos1, &maxpos1,100000);
00074 printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1);
00075 kmot_SearchLimits(motor2, 1, 3, &minpos2, &maxpos2,100000);
00076 printf("motor2: min:%ld max:%ld\n\r",minpos2, maxpos2);
00077 kmot_SearchLimits(motor3, 5, 3, &minpos3, &maxpos3,100000);
00078 printf("motor3: min:%ld max:%ld\n\r",minpos3, maxpos3);
00079 kmot_SetBlockedTime(motor0,5);
00080 kmot_SetBlockedTime(motor2,5);
00081
00082 return NULL;
00083 }
00084
00085
00086
00089 int koa_net_init(int argc, char * argv[])
00090 {
00091 pthread_t init_task;
00092 int rc;
00093
00094 rc = pthread_create(&init_task, NULL, koa_init_task, NULL);
00095 if (rc){
00096 KB_FATAL("koa_net_init", KB_ERROR_PTHREAD, "koa_init_task");
00097 }
00098 }
00099
00100 int koa_net_camera(int argc, char * argv[])
00101 {
00102 int i;
00103 int pos0,pos1;
00104
00105 pos0 = atoi(argv[1]);
00106 if(pos0 < 5)
00107 pos0 = 6;
00108 if(pos0 > 95)
00109 pos0 = 95;
00110
00111 pos1 = atoi(argv[2]);
00112 if(pos1 < 5)
00113 pos1 = 6;
00114 if(pos1 > 95)
00115 pos1 = 95;
00116
00117 pos0 = (pos0 * minpos0)/100;
00118 pos1 = (pos1 * minpos1)/100;
00119 #if 0
00120 kmot_SetPoint(motor0, kMotRegPos, pos0);
00121 kmot_SetPoint(motor1, kMotRegPos, pos1);
00122 #else
00123 kmot_SetPoint(motor0, kMotRegPosProfile, pos0);
00124 kmot_SetPoint(motor1, kMotRegPosProfile, pos1);
00125 #endif
00126 for(i=0;i<argc;i++)
00127 printf("%s ",argv[i]);
00128 printf("(%d,%d)\r\n",pos0,pos1);
00129 }
00130
00131 int koa_net_camera2(int argc, char * argv[])
00132 {
00133 int i;
00134 int pos0,pos1;
00135
00136 pos0 = atoi(argv[1]);
00137 if(pos0 < 5)
00138 pos0 = 6;
00139 if(pos0 > 95)
00140 pos0 = 95;
00141
00142 pos1 = atoi(argv[2]);
00143 if(pos1 < 5)
00144 pos1 = 6;
00145 if(pos1 > 95)
00146 pos1 = 95;
00147
00148 pos0 = (pos0 * minpos2)/100;
00149 pos1 = (pos1 * minpos3)/100;
00150 kmot_SetPoint(motor2, kMotRegPos, pos0);
00151 kmot_SetPoint(motor3, kMotRegPos, pos1);
00152 for(i=0;i<argc;i++)
00153 printf("%s ",argv[i]);
00154 printf("(%d,%d)\r\n",pos0,pos1);
00155 }
00156
00157 int koa_net_bothcam(int argc, char * argv[])
00158 {
00159 int i;
00160 int pos0,pos1;
00161
00162 pos0 = atoi(argv[1]);
00163 if(pos0 < 5)
00164 pos0 = 6;
00165 if(pos0 > 95)
00166 pos0 = 95;
00167
00168 pos1 = atoi(argv[2]);
00169 if(pos1 < 5)
00170 pos1 = 6;
00171 if(pos1 > 95)
00172 pos1 = 95;
00173
00174 kmot_SetPoint(motor0, kMotRegPos, (pos0 * minpos0)/100);
00175 kmot_SetPoint(motor1, kMotRegPos, (pos1 * minpos1)/100);
00176 kmot_SetPoint(motor2, kMotRegPos, (pos0 * minpos2)/100);
00177 kmot_SetPoint(motor3, kMotRegPos, (pos1 * minpos3)/100);
00178 for(i=0;i<argc;i++)
00179 printf("%s ",argv[i]);
00180 printf("(%d,%d)\r\n",pos0,pos1);
00181 }
00182
00183 int koa_net_stop(int argc, char * argv[])
00184 {
00185 koa_setSpeed(koala,0,0);
00186
00187 return 0;
00188 }
00189
00190 int koa_net_setspeed(int argc, char * argv[])
00191 {
00192 int sensor_table[16];
00193 int sum;
00194 int newright,newleft;
00195 int i;
00196
00197 right = atoi(argv[1]);
00198 left = atoi(argv[2]);
00199
00200 newright = atoi(argv[1]);
00201 newleft = atoi(argv[2]);
00202
00203 if(newleft > 0)
00204 {
00205
00206 if(left_limit_p<newleft)
00207 newleft = left_limit_p;
00208 }
00209 else
00210 if(newleft < 0)
00211 {
00212
00213 if(left_limit_n>newleft)
00214 newleft = left_limit_n;
00215 }
00216
00217 if(newright > 0)
00218 {
00219
00220 if(right_limit_p<newright)
00221 newright = right_limit_p;
00222 }
00223 else
00224 if(newright < 0)
00225 {
00226
00227 if(right_limit_n>newright)
00228 newright = right_limit_n;
00229 }
00230
00231 koa_setSpeed(koala,newright,newleft);
00232
00233 for(i=0;i<argc;i++)
00234 printf("%s ",argv[i]);
00235 printf("(%d,%d)->(%d,%d)\r\n",right,left,newright,newleft);
00236
00237 return 0;
00238 }
00239
00240 int net_exit(int argc, char * argv[])
00241 {
00242 exit(0);
00243 }
00244
00245
00248 int koa_net_playsound(int argc, char * argv[])
00249 {
00250 unsigned sound = atoi(argv[1]);
00251 koasound_t * newsound;
00252
00253 if(sound >= soundlist_size)
00254 return 1;
00255
00256 newsound = kb_alloc(sizeof(koasound_t));
00257 newsound->sound = sound;
00258 newsound->next = NULL;
00259
00260
00261 pthread_mutex_lock(&soundlock);
00262 if(lastsound != NULL)
00263 lastsound->next = newsound;
00264 else
00265 currentsound = newsound;
00266 lastsound = newsound;
00267 pthread_mutex_unlock(&soundlock);
00268
00269 printf("Addsound %d\r\n", sound);
00270
00271 return 0;
00272 }
00273
00274
00277 #define soundbufsize 2048
00278 void * koa_sound_task(void * arg)
00279 {
00280 WAV wav;
00281 int rc;
00282 snd_t snd;
00283 short buf[soundbufsize];
00284 int nextsound;
00285 koasound_t * previous;
00286
00287
00288 kb_snd_init(&snd , "/dev/sound/dsp", "/dev/sound/mixer");
00289
00290 if ( kb_snd_open(&snd ) < 0 ) {
00291 printf("cannot open sound devices\r\n");
00292 return NULL;
00293 }
00294
00295 kb_wav_init( &wav );
00296
00297
00298 for(;;)
00299 if(currentsound != NULL)
00300 {
00301 nextsound = currentsound->sound;
00302 printf("Playsound %d\r\n", nextsound);
00303
00304 if ((rc = kb_wav_open( &wav , soundlist[nextsound], 1 )) == -1 ) {
00305 printf("cannot open %s\r\n",soundlist[nextsound]);
00306 continue;
00307 }
00308
00309 if ( wav.fmt.bit_per_sample != 16 && wav.fmt.nb_channel != 2 ) {
00310 printf("wrong bit_per_sample or nb_channel in wav file\r\n");
00311 continue;
00312 }
00313
00314
00315 if ( kb_snd_setSampleRate( &snd, wav.fmt.sample_rate ) == -1 ) {
00316 printf("unable to set sample rate\r\n");
00317 continue;
00318 }
00319
00320
00321 for (;;) {
00322 rc = kb_wav_read_data( &wav , buf , soundbufsize);
00323 if ( rc <= 0 ) {
00324 if ( rc < 0 )
00325 printf( "i/o error in reading file '%s' !" , soundlist[nextsound]);
00326 break;
00327 }
00328
00329 do {
00330 rc = kb_snd_play( &snd , buf , soundbufsize);
00331 if ( rc < 0 ) {
00332 if ( errno != EAGAIN ) {
00333 printf( "error in playing sample errno=%d", errno );
00334 break;
00335 }
00336 continue;
00337 }
00338 }
00339 while (0);
00340 }
00341
00342 kb_wav_close( &wav );
00343
00344
00345 pthread_mutex_lock(&soundlock);
00346 previous = currentsound;
00347 currentsound = currentsound->next;
00348 if(currentsound == NULL)
00349 lastsound = NULL;
00350 pthread_mutex_unlock(&soundlock);
00351
00352 if(previous != NULL)
00353 kb_free(previous);
00354 }
00355 }
00356
00357
00360 void * koa_sensor_task(void * arg)
00361 {
00362 int sensor_table[16];
00363 int sum;
00364 int newright,newleft;
00365
00366 for(;;)
00367 {
00368 newright = right;
00369 newleft = left;
00370
00371 koa_readProximity(koala,sensor_table);
00372
00373 if(newright > 0)
00374 {
00375
00376 sum = sensor_table[0] + 2*sensor_table[8] + 3*sensor_table[9] +
00377 sensor_table[12] + 2*sensor_table[11] + 3*sensor_table[10];
00378 sum = (sum / 12) - 50;
00379 if(sum<0)
00380 sum = 0;
00381 sum = (sum*100) / 250;
00382 if(sum>100)
00383 sum = 100;
00384 sum = 100 - sum;
00385 right_limit_p = sum;
00386 if(sum<newright)
00387 newright = sum;
00388 }
00389 else
00390 {
00391
00392 sum = sensor_table[13] + 2*sensor_table[14] + sensor_table[15];
00393 sum = (sum / 4) - 50;
00394 if(sum<0)
00395 sum = 0;
00396 sum = (sum*100) / 250;
00397 if(sum>100)
00398 sum = 100;
00399 sum = -(100 - sum);
00400 right_limit_n = sum;
00401 if(sum>newright)
00402 newright = sum;
00403 }
00404
00405 if(newleft > 0)
00406 {
00407
00408 sum = sensor_table[8] + 2*sensor_table[0] + 3*sensor_table[1] +
00409 sensor_table[4] + 2*sensor_table[3] + 3*sensor_table[2];
00410 sum = (sum / 12) - 50;
00411 if(sum<0)
00412 sum = 0;
00413 sum = (sum*100) / 250;
00414 if(sum>100)
00415 sum = 100;
00416 sum = 100 - sum;
00417 left_limit_p = sum;
00418 if(sum<newleft)
00419 newleft = sum;
00420 }
00421 else
00422 {
00423
00424 sum = sensor_table[5] + 2*sensor_table[6] + sensor_table[7];
00425 sum = (sum / 4) - 50;
00426 if(sum<0)
00427 sum = 0;
00428 sum = (sum*100) / 250;
00429 if(sum>100)
00430 sum = 100;
00431 sum = -(100 - sum);
00432 left_limit_n = sum;
00433 if(sum>newleft)
00434 newleft = sum;
00435 }
00436 koa_setSpeed(koala,newright,newleft);
00437 }
00438 }
00439
00440
00443 void InitMotor(knet_dev_t * mot)
00444 {
00445 kmot_SetMode(mot,0);
00446 kmot_SetSampleTime(mot,6);
00447 kmot_SetMargin(mot,50);
00448 kmot_SetOptions(mot,0,kMotSWOptWindup|kMotSWOptStopMotorBlk);
00449 kmot_ResetError(mot);
00450
00451 kmot_ConfigurePID(mot,kMotRegSpeed,500,250,30);
00452 kmot_ConfigurePID(mot,kMotRegPos,70,50,5);
00453 kmot_SetSpeedProfile(mot,20,100);
00454
00455 kmot_SetBlockedTime(mot,5);
00456 kmot_SetLimits(mot,kMotRegCurrent,0,50);
00457 kmot_SetLimits(mot,kMotRegPos,-10000,10000);
00458 }
00459
00460
00461 int main( int argc , char * argv[] )
00462 {
00463 ksock_t server;
00464 int rc;
00465 unsigned char buf[16];
00466 unsigned char a,b,c,d;
00467 int clntSocket;
00468 char echoBuffer[RCVBUFSIZE];
00469 int recvMsgSize;
00470 int port;
00471
00472 pthread_t sensor_task;
00473 pthread_t sound_task;
00474
00475 printf("K-Team Koala Demo Program - Server side\r\n");
00476
00477 if(argc > 1)
00478 port = atoi(argv[1]);
00479 else
00480 port = 344;
00481
00482
00483
00484 kb_set_debug_level( 2 );
00485
00486
00487 if((rc = kb_init( argc , argv )) < 0 )
00488 return 1;
00489
00490
00491 if((rc = ksock_init('\n')) < 0 )
00492 return 1;
00493
00494
00495 koala = knet_open( "Koala:Robot", KNET_BUS_ANY, 0 , NULL );
00496 if(!koala)
00497 {
00498 printf("Open Koala device failed\r\n");
00499 return 1;
00500 }
00501
00502 motor1 = knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL );
00503 if(!motor1)
00504 {
00505 printf("Cannot open motor 0\r\n");
00506 return 1;
00507 }
00508 motor0 = knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL );
00509 if(!motor0)
00510 {
00511 printf("Cannot open motor 1\r\n");
00512 return 1;
00513 }
00514 motor3 = knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL );
00515 if(!motor3)
00516 {
00517 printf("Cannot open motor 2\r\n");
00518 return 1;
00519 }
00520 motor2 = knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL );
00521 if(!motor2)
00522 {
00523 printf("Cannot open motor 3\r\n");
00524 return 1;
00525 }
00526
00527
00528
00529 koa_getOSVersion(koala,&a,&b,&c,&d);
00530 printf("OS revision %d.%d protocol %d.%d\r\n",a,b,c,d);
00531 koa_setSpeed(koala,0,0);
00532
00533 kmot_GetFWVersion( motor1, &rc);
00534 printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(rc) , KMOT_REVISION(rc));
00535
00536
00537 InitMotor(motor0);
00538 InitMotor(motor1);
00539 InitMotor(motor2);
00540 InitMotor(motor3);
00541 kmot_SearchLimits(motor0, 1, 3, &minpos0, &maxpos0,100000);
00542 printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0);
00543 kmot_SearchLimits(motor1, 5, 3, &minpos1, &maxpos1,100000);
00544 printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1);
00545 kmot_SearchLimits(motor2, 1, 3, &minpos2, &maxpos2,100000);
00546 printf("motor2: min:%ld max:%ld\n\r",minpos2, maxpos2);
00547 kmot_SearchLimits(motor3, 5, 3, &minpos3, &maxpos3,100000);
00548 printf("motor3: min:%ld max:%ld\n\r",minpos3, maxpos3);
00549 kmot_SetBlockedTime(motor0,5);
00550 kmot_SetBlockedTime(motor2,5);
00551
00552
00553 ksock_server_open(&server, port);
00554
00555 ksock_add_command("setspeed",2,2,koa_net_setspeed);
00556 ksock_add_command("stop",0,0,koa_net_stop);
00557 ksock_add_command("movecamera",2,2,koa_net_camera);
00558 ksock_add_command("movecamera2",2,2,koa_net_camera2);
00559 ksock_add_command("moveboth",2,2,koa_net_bothcam);
00560 ksock_add_command("playsound",1,1,koa_net_playsound);
00561 ksock_add_command("initcamera",0,0,koa_net_init);
00562 list_command();
00563
00564
00565 rc = pthread_create(&sensor_task, NULL, koa_sensor_task, NULL);
00566 if (rc){
00567 printf("ERROR; return code from pthread_create() is %d\n", rc);
00568 exit(-1);
00569 }
00570 rc = pthread_create(&sound_task, NULL, koa_sound_task, NULL);
00571 if (rc){
00572 printf("ERROR; return code from pthread_create() is %d\n", rc);
00573 exit(-1);
00574 }
00575
00576
00577 clntSocket = ksock_next_connection(&server);
00578 for(;;)
00579 {
00580 rc = ksock_get_command(clntSocket, echoBuffer, RCVBUFSIZE);
00581
00582 if(rc>0)
00583 {
00584 ksock_exec_command(echoBuffer);
00585
00586 if (send(clntSocket, echoBuffer, rc, 0) != rc)
00587 DieWithError("send() failed");
00588 }
00589 else
00590 switch(rc)
00591 {
00592 case 0 :
00593 break;
00594 case -3 :
00595 printf("Client disconnected\r\n");
00596 clntSocket = ksock_next_connection(&server);
00597 break;
00598 default :
00599 printf("Socket error: %d\r\n",rc);
00600 clntSocket = ksock_next_connection(&server);
00601 break;
00602 }
00603 }
00604
00605 close(clntSocket);
00606
00607 return 0;
00608 }
00609