Subversion Repositories svnkaklik

Rev

Rev 379 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 379 Rev 380
Line 57... Line 57...
57 
double nord, east;
 57 
double nord, east;
58 
int done; // vlajka, ze se neco udelalo
 58 
int done; // vlajka, ze se neco udelalo
59 
int last_cross; // posledni krizovatka
 59 
int last_cross; // posledni krizovatka
60 
int korekce; // Byla korekce podle kompasu nebo zdi
 60 
int korekce; // Byla korekce podle kompasu nebo zdi
61 
int bod;
 61 
int bod;
- 
 
 62 
int start;
62 
 
 63 
 
63 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 64 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
64 
 
 65 
 
65 
void *print_tele(void *unused);
 66 
void *print_tele(void *unused);
66 
void *gps(void *unused);
 67 
void *gps(void *unused);
Line 179... Line 180...
179 
    unsigned char azimut;
 180 
    unsigned char azimut;
180 
 
 181 
 
181 
    pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
 182 
    pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
182 
    pomN=nord; pomE=east;
 183 
    pomN=nord; pomE=east;
183 
    pthread_mutex_unlock(&mutex);
 184 
    pthread_mutex_unlock(&mutex);
- 
 
 185 
korekce=0;
184 
//    if (last_cross!=-1) if ( GeoCalc::EllipsoidDistance(pomN, pomE, cros[last_cross].n, cros[last_cross].e)>((double)cros[n].dia+5) ) done=0; // znovu naviguj, pokud si dal nez... od krizovatky
 186 
//    if (last_cross!=-1) if ( GeoCalc::EllipsoidDistance(pomN, pomE, cros[last_cross].n, cros[last_cross].e)>((double)cros[n].dia+5) ) done=0; // znovu naviguj, pokud si dal nez... od krizovatky
185 
    n=0;
 187 
    n=0;
186 
    do
 188 
    do
187 
    {
 189 
    {
188 
        if (0==cros[n].id) break;
 190 
        if (0==cros[n].id) break;
189 
        dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e);
 191 
        dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e);
190 
        if (dist <= (double)cros[n].dia)
 192 
        if (dist <= (double)cros[n].dia)
191 
        {
 193 
        {
192 
//printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
 194 
//printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
- 
 
 195 
korekce=1;
193 
            if (last_cross!=cros[n].id)
 196 
            if (last_cross!=cros[n].id)
194 
            {
 197 
            {
195 
                bod=cros[n].id;
 198 
                bod=cros[n].id;
196 
                #include "nav.h"
 199 
                #include "nav.h"
197 
//                done=1;
 200 
//                done=1;
Line 240... Line 243...
240 
 
 243 
 
241 
void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel (otaceni v levo)
 244 
void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel (otaceni v levo)
242 
{
 245 
{
243 
int azimut;
 246 
int azimut;
244 
 
 247 
 
245 
    go(M1, -speed);
 248 
    go(M1, 0);
246 
    go(M2, speed);
 249 
    go(M2, speed);
247 
    azimut=read_azimut_mag();
 250 
    azimut=read_azimut_mag();
- 
 
 251 
//    if((azimut-angle)>192) {azimut+=64; angle+=64};
248 
    while(read_azimut_mag() >= (azimut - angle)) usleep(10000);
 252 
    while(read_azimut_mag() >= (azimut - angle)) usleep(10000);
249 
    go(M1, 0);
 253 
    go(M1, 0);
250 
    go(M2, 0);
 254 
    go(M2, 0);
251 
}
 255 
}
252 
 
 256 
 
Line 283... Line 287...
283 
 
 287 
 
284 
    while(true)
 288 
    while(true)
285 
    {
 289 
    {
286 
        switch (command)
 290 
        switch (command)
287 
        {
 291 
        {
- 
 
 292 
            case 'o':   // forward
- 
 
 293 
                start=1;
- 
 
 294 
                command=0;
- 
 
 295 
                break;
- 
 
 296 
 
288 
            case 'f':   // forward
 297 
            case 'f':   // forward
289 
                go(M1, param);
 298 
                go(M1, param);
290 
                go(M2, param);
 299 
                go(M2, param);
291 
                sleep(1);
 300 
                sleep(1);
292 
                command=0;
 301 
                command=0;
Line 310... Line 319...
310 
                go(M2, 0);
 319 
                go(M2, 0);
311 
                command=0;
 320 
                command=0;
312 
                break;
 321 
                break;
313 
 
 322 
 
314 
            case 'l':   // left
 323 
            case 'l':   // left
- 
 
 324 
    turnL(20, 120);    // otoci robota o zadany uhel (otaceni v levo)
315 
                TL(param);
 325 
//                TL(param);
316 
                command=0;
 326 
                command=0;
317 
                break;
 327 
                break;
318 
 
 328 
 
319 
            case 'r':   // right
 329 
            case 'r':   // right
320 
                TR(param);
 330 
                TR(param);
Line 346... Line 356...
346 
                command=0;
 356 
                command=0;
347 
                break;
 357 
                break;
348 
 
 358 
 
349 
            case 'g':
 359 
            case 'g':
350 
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
 360 
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
- 
 
 361 
if(start==1)
351 
 
 362 
{
- 
 
 363 
    go(M1, 127);
- 
 
 364 
    go(M2, 127);
- 
 
 365 
    sleep(10);
- 
 
 366 
    turnL(5, 120);    // otoci robota o zadany uhel (otaceni v levo)
- 
 
 367 
    go(M1, 127);
- 
 
 368 
    go(M2, 127);
- 
 
 369 
    sleep(10);
- 
 
 370 
    turnL(5, 120);    // otoci robota o zadany uhel (otaceni v levo)
- 
 
 371 
    go(M1, 127);
- 
 
 372 
    go(M2, 127);
- 
 
 373 
    sleep(5);
- 
 
 374 
//    turnL(5, 120);    // otoci robota o zadany uhel (otaceni v levo)
- 
 
 375 
    go(M1, 127);
- 
 
 376 
    go(M2, 127);
- 
 
 377 
    sleep(10);
- 
 
 378 
   start=0;
- 
 
 379 
};
352 
                FindNearestCrossing();
 380 
//                FindNearestCrossing();
353 
//!!!                FindWay();
 381 
//!!!                FindWay();
354 
 
 382 
 
355 
                vzdalenost=echo(US3_Addr);
 383 
                vzdalenost=echo(US3_Addr);
356 
                usleep(30000);
 384 
                usleep(30000);
357 
                lus=echo(US1_Addr);
 385 
                lus=echo(US1_Addr);
358 
                usleep(30000);
 386 
                usleep(30000);
359 
                rus=echo(US2_Addr);
 387 
                rus=echo(US2_Addr);
360 
                usleep(30000);
 388 
                usleep(30000);
361 
 
 389 
 
362 
                if ((vzdalenost>60)&&(vzdalenost<80))
 390 
if ((vzdalenost<30)||(vzdalenost>150))
- 
 
 391 
{
- 
 
 392 
    go(M1, 0);  // zastav, neni videt doprovod
363 
                {
 393 
    go(M2, 0);
- 
 
 394 
    break;
364 
 
 395 
};
365 
//FindNearestCrossing();
 - 
 
366 
 
 396 
 
367 
//                    if(korekce==0)
 - 
 
368 
                    {
 - 
 
369 
                        go(M1, 80);
 - 
 
370 
                        go(M2, 80);
 - 
 
371 
                    };
 - 
 
372 
                    korekce=0;
 - 
 
373 
                    filtr=0;
 - 
 
374 
                    break;
 - 
 
375 
                };
 - 
 
376 
                if ((vzdalenost>30)&&(vzdalenost<130))
 397 
                if ((vzdalenost>60)&&(vzdalenost<100))
377 
                {
 398 
                {
378 
/*
 - 
 
379 
                    if ((lus<70)&&(lus>30)&&(vzdalenost>60)) // prekazka vlevo
 - 
 
380 
                    {
 - 
 
381 
                        go(M1, 60);
 - 
 
382 
                        go(M2, 40);
 - 
 
383 
                        break;
 - 
 
384 
                    };
 - 
 
385 
 
 399 
 
386 
*/
 - 
 
387 
/*
 - 
 
388 
                    if ((rus<40)&&(rus>30)) // prekazka vpravo
 400 
//FindNearestCrossing();
- 
 
 401 
//if(korekce==0)
389 
                    {
 402 
                    {
390 
                        go(M1, 40);
 403 
                        go(M1, 40);
391 
                        go(M2, 60);
 404 
                        go(M2, 60);
- 
 
 405 
                        break;
392 
                    };
 406 
                    };
- 
 
 407 
//                   filtr=0;
- 
 
 408 
                };
- 
 
 409 
                if ((vzdalenost>=100))
393 
*/
 410 
                {
394 
 
 - 
 
395 
                    filtr=0;
 411 
                    filtr=0;
396 
/*
 - 
 
397 
                    if (vzdalenost<40)
 - 
 
398 
                    {
 - 
 
399 
                        go(M1, 40);
 - 
 
400 
                        go(M2, 60);
 - 
 
401 
                        break;
 - 
 
402 
                    }
 - 
 
403 
                    if (vzdalenost>100)
 - 
 
404 
                    {
 - 
 
405 
                        go(M1, 60);
 412 
                    go(M1, 60);
406 
                        go(M2, 40);
 413 
                    go(M2, 40);
407 
                        break;
 414 
                    break;
408 
                    }
 - 
 
409 
*/
 - 
 
410 
                    if (vzdalenost<55)
 - 
 
411 
                    {
 - 
 
412 
                        go(M1, 40);
 - 
 
413 
                        go(M2, 60);
 - 
 
414 
                    }
 - 
 
415 
                    else
 - 
 
416 
                    {
 - 
 
417 
                        go(M1, 60);
 - 
 
418 
                        go(M2, 40);
 - 
 
419 
                    }
 - 
 
420 
                }
 415 
                }
421 
                else
 - 
 
422 
                {
 - 
 
423 
                    filtr++;
 - 
 
424 
                    if (filtr>5)
 - 
 
425 
                    {
 - 
 
426 
                        go(M1, 0);  // zastav, neni videt doprovod
 - 
 
427 
                        go(M2, 0);
 - 
 
428 
                        filtr=6;
 - 
 
429 
                    }
 - 
 
430 
                };
 - 
 
431 
                break;
 416 
                break;
432 
        }
 417 
        }
433 
    };
 418 
    };
434 
 
 419 
 
435 
	close(file);
 420 
	close(file);