Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 379 → Rev 380

/roboti/Robotour/SW/vector/nav.h
1,28 → 1,36
switch (cros[n].id)
{
case 1:
// TL(20);
azimut=read_azimut_mag();
if ((azimut>0)&&(azimut<45))
if ((azimut>0)&&(azimut<100))
{
if(azimut>20) SlowLeft(); else SlowRight();
if(azimut>0) SlowLeft(); else SlowRight();
korekce=1;
};
// last_cross=cros[n].id;
break;
case 10:
TR(250);
last_cross=cros[n].id;
case 2:
azimut=read_azimut_mag();
if ((azimut>0)&&(azimut<80))
{
if(azimut>0) SlowLeft(); else SlowRight();
korekce=1;
};
break;
case 3:
TR(195);
last_cross=cros[n].id;
azimut=read_azimut_mag();
if ((azimut>0)&&(azimut<100))
{
if(azimut>40) SlowLeft(); else SlowRight();
korekce=1;
};
break;
case 4:
// TL(194);
if ((lus>100)&&(lus<300)) SlowLeft();
azimut=read_azimut_mag();
if ((azimut>0)&&(azimut<100))
{
if(azimut>45) SlowLeft(); else SlowRight();
korekce=1;
// last_cross=cros[n].id;
};
break;
case 5:
TL(176);
/roboti/Robotour/SW/vector/track.h
12,6 → 12,13
//{2,50.105177,14.427805,6},
 
//{1,50.105460, 14.428012,20},
{1, 50.105347, 14.427978,20},
//{2, 50.105635, 14.427998,20}, // dal
{2, 50.105573, 14.428026, 20}, // bliz
{3, 50.105810, 14.427848,20},
{4, 50.105944, 14.427608,20},
 
//{1, 50.105225, 14.427975,20},
//{2, 50.105723, 14.427923,20},
//{3, 50.105872, 14.427731,20},
/*
/roboti/Robotour/SW/vector/vector.cpp
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;
}
};