| 59,6 → 59,7 |
|---|
| int last_cross; // posledni krizovatka |
| int korekce; // Byla korekce podle kompasu nebo zdi |
| int bod; |
| int start; |
| |
| pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; |
| |
| 181,6 → 182,7 |
|---|
| pthread_mutex_lock(&mutex); // prepis souradnic do pracovnich promennych |
| pomN=nord; pomE=east; |
| pthread_mutex_unlock(&mutex); |
| korekce=0; |
| // 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 |
| n=0; |
| do |
| 190,6 → 192,7 |
|---|
| if (dist <= (double)cros[n].dia) |
| { |
| //printf("Point ID: %d - Distance: %f\n",cros[n].id, dist); |
| korekce=1; |
| if (last_cross!=cros[n].id) |
| { |
| bod=cros[n].id; |
| 242,9 → 245,10 |
|---|
| { |
| int azimut; |
| |
| go(M1, -speed); |
| go(M1, 0); |
| go(M2, speed); |
| azimut=read_azimut_mag(); |
| // if((azimut-angle)>192) {azimut+=64; angle+=64}; |
| while(read_azimut_mag() >= (azimut - angle)) usleep(10000); |
| go(M1, 0); |
| go(M2, 0); |
| 285,6 → 289,11 |
|---|
| { |
| switch (command) |
| { |
| case 'o': // forward |
| start=1; |
| command=0; |
| break; |
| |
| case 'f': // forward |
| go(M1, param); |
| go(M2, param); |
| 312,7 → 321,8 |
|---|
| break; |
| |
| case 'l': // left |
| TL(param); |
| turnL(20, 120); // otoci robota o zadany uhel (otaceni v levo) |
| // TL(param); |
| command=0; |
| break; |
| |
| 348,8 → 358,26 |
|---|
| |
| case 'g': |
| usleep(180000); // simulace ostatnich cidel (zdrzeni) |
| |
| FindNearestCrossing(); |
| if(start==1) |
| { |
| go(M1, 127); |
| go(M2, 127); |
| sleep(10); |
| turnL(5, 120); // otoci robota o zadany uhel (otaceni v levo) |
| go(M1, 127); |
| go(M2, 127); |
| sleep(10); |
| turnL(5, 120); // otoci robota o zadany uhel (otaceni v levo) |
| go(M1, 127); |
| go(M2, 127); |
| sleep(5); |
| // turnL(5, 120); // otoci robota o zadany uhel (otaceni v levo) |
| go(M1, 127); |
| go(M2, 127); |
| sleep(10); |
| start=0; |
| }; |
| // FindNearestCrossing(); |
| //!!! FindWay(); |
| |
| vzdalenost=echo(US3_Addr); |
| 359,75 → 387,32 |
|---|
| rus=echo(US2_Addr); |
| usleep(30000); |
| |
| if ((vzdalenost>60)&&(vzdalenost<80)) |
| if ((vzdalenost<30)||(vzdalenost>150)) |
| { |
| go(M1, 0); // zastav, neni videt doprovod |
| go(M2, 0); |
| break; |
| }; |
| |
| if ((vzdalenost>60)&&(vzdalenost<100)) |
| { |
| |
| //FindNearestCrossing(); |
| |
| // if(korekce==0) |
| //if(korekce==0) |
| { |
| go(M1, 80); |
| go(M2, 80); |
| go(M1, 40); |
| go(M2, 60); |
| break; |
| }; |
| korekce=0; |
| filtr=0; |
| break; |
| // filtr=0; |
| }; |
| if ((vzdalenost>30)&&(vzdalenost<130)) |
| if ((vzdalenost>=100)) |
| { |
| /* |
| if ((lus<70)&&(lus>30)&&(vzdalenost>60)) // prekazka vlevo |
| { |
| go(M1, 60); |
| go(M2, 40); |
| break; |
| }; |
| |
| */ |
| /* |
| if ((rus<40)&&(rus>30)) // prekazka vpravo |
| { |
| go(M1, 40); |
| go(M2, 60); |
| }; |
| */ |
| |
| filtr=0; |
| /* |
| if (vzdalenost<40) |
| { |
| go(M1, 40); |
| go(M2, 60); |
| break; |
| } |
| if (vzdalenost>100) |
| { |
| go(M1, 60); |
| go(M2, 40); |
| break; |
| } |
| */ |
| if (vzdalenost<55) |
| { |
| go(M1, 40); |
| go(M2, 60); |
| } |
| else |
| { |
| go(M1, 60); |
| go(M2, 40); |
| } |
| go(M1, 60); |
| go(M2, 40); |
| break; |
| } |
| else |
| { |
| filtr++; |
| if (filtr>5) |
| { |
| go(M1, 0); // zastav, neni videt doprovod |
| go(M2, 0); |
| filtr=6; |
| } |
| }; |
| break; |
| } |
| }; |