Subversion Repositories svnkaklik

Compare Revisions

Regard whitespace Rev 378 → Rev 379

/roboti/Robotour/SW/vector/nav.h
1,8 → 1,48
switch (cros[n].id)
{
case 1:
TL(200);
// TL(20);
azimut=read_azimut_mag();
if ((azimut>0)&&(azimut<45))
{
if(azimut>20) SlowLeft(); else SlowRight();
korekce=1;
};
// last_cross=cros[n].id;
break;
case 2:
case 10:
TR(250);
last_cross=cros[n].id;
break;
case 3:
TR(195);
last_cross=cros[n].id;
break;
case 4:
// TL(194);
if ((lus>100)&&(lus<300)) SlowLeft();
korekce=1;
// last_cross=cros[n].id;
break;
case 5:
TL(176);
last_cross=cros[n].id;
break;
case 6:
TL(168);
last_cross=cros[n].id;
break;
case 7:
TR(230);
last_cross=cros[n].id;
break;
case 8:
TL(38);
last_cross=cros[n].id;
break;
case 9:
TL(1);
last_cross=cros[n].id;
break;
};
 
/roboti/Robotour/SW/vector/track.h
8,8 → 8,23
unsigned int dia;
} cros [POINTS] =
{
{1,49.259562, 14.705632,2},
{2,49.835265, 18.161128,5},
//{1,50.105205, 14.427857,6},
//{2,50.105177,14.427805,6},
 
//{1,50.105460, 14.428012,20},
//{2, 50.105723, 14.427923,20},
//{3, 50.105872, 14.427731,20},
/*
 
{4, 50.105975, 14.427224,30},
{5, 50.105651, 14.427026,10},
 
{6, 50.105481, 14.427033,10},
*/
{7, 50.105218, 14.427115,10},
{8, 50.105403, 14.426855,10},
{9, 50.105836, 14.426683,10},
{10, 50.105826, 14.427683,10},
{0,0,0,0}
};
 
19,8 → 34,10
double n;
double e;
unsigned int dia;
unsigned char azimut;
int smer;// 1=levo, 2=pravo
} way [POINTS] =
{
{1,49.835265, 18.161128,5},
{1,49.835265, 18.161128,5, 200,1},
{0,0,0,0}
};
/roboti/Robotour/SW/vector/vector.cpp
50,7 → 50,7
char vystup[50];
pthread_t thread_1, thread_2, thread_3;
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
unsigned int vzdalenost, lus, rus;
char command,ble;
int param;
int file;
57,6 → 57,8
double nord, east;
int done; // vlajka, ze se neco udelalo
int last_cross; // posledni krizovatka
int korekce; // Byla korekce podle kompasu nebo zdi
int bod;
 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
64,7 → 66,7
void *gps(void *unused);
//void *sensors(void *unused);
 
void I2C_addr (int Addr)
void I2C_addr (int Addr) // vybere adresu cidla se kterym se bude komunikovat
{
if (ioctl(file, I2C_SLAVE, Addr) == -1)
{
73,6 → 75,17
}
}
 
int i2c_init() // zinicializuje i2c
{
file = open("/dev/i2c-0", O_RDWR);
if (file < 0)
{
cerr << "Could not open /dev/i2c-0." << endl;
return -1;
}
return 0;
}
 
void go (int Addr, int speed) // nastavi rychlost motoru
{
char Buf[1];
116,7 → 129,7
write(file,Buf,2);
}
 
void TL (unsigned char azimut2)
void TL (unsigned char azimut2) // otoci robota na zadany azimut rotaci vlevo
{
unsigned char azimut1;
 
127,12 → 140,12
azimut1=read_azimut_mag();
//printf("az1: %d - az2: %d\n", azimut1, azimut2);
usleep(10000);
} while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2) );
} while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2)&&((azimut1+4)!=azimut2) );
go(M1, 0);
go(M2, 0);
}
 
void TR (unsigned char azimut2)
void TR (unsigned char azimut2) // otoci robota na zadany azimut rotaci v pravo
{
unsigned char azimut1;
 
142,20 → 155,33
{
azimut1=read_azimut_mag();
usleep(10000);
} while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2) );
} while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2)&&((azimut1-4)!=azimut2) );
go(M1, 0);
go(M2, 0);
}
 
void FindNearestCrossing(void)
void SlowLeft()
{
go(M1, 40);
go(M2, 60);
}
 
void SlowRight()
{
go(M1, 60);
go(M2, 40);
}
 
void FindNearestCrossing(void) // vykonej nejakou akci (zatoceni) pokud se robot nachazi pobliz nejakeho waypointu
{
int n;
double dist, pomN, pomE;
unsigned char azimut;
 
pthread_mutex_lock(&mutex); // prepis souradnic do pracovnich promennych
pomN=nord; pomE=east;
pthread_mutex_unlock(&mutex);
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
// 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
{
164,11 → 190,11
if (dist <= (double)cros[n].dia)
{
//printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
if (done==0)
if (last_cross!=cros[n].id)
{
last_cross=n;
bod=cros[n].id;
#include "nav.h"
done=1;
// done=1;
}
break;
}
175,21 → 201,59
} while(++n<POINTS);
}
 
int i2c_init() // zinicializuje i2c
/*
void FindWay(void) // koriguj smer podle kompasu
{
file = open("/dev/i2c-0", O_RDWR);
if (file < 0)
int n;
double dist, pomN, pomE;
 
pthread_mutex_lock(&mutex); // prepis souradnic do pracovnich promennych
pomN=nord; pomE=east;
pthread_mutex_unlock(&mutex);
// 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
{
cerr << "Could not open /dev/i2c-0." << endl;
return -1;
if (0==way[n].id) break;
dist=GeoCalc::EllipsoidDistance(pomN, pomE, way[n].n, way[n].e);
if (dist <= (double)way[n].dia)
{
if (last_way!=way[n].id)
{
last_way=way[n].id;
switch (way[n].id)
{
case 1:
TL(way[n].azimut);
break;
case 2:
TL(80);
break;
};
 
}
return 0;
break;
}
} while(++n<POINTS);
}
*/
 
void turnL(unsigned char angle, signed char speed) // otoci robota o zadany uhel
void turnL(unsigned char angle, signed char speed) // otoci robota o zadany uhel (otaceni v levo)
{
int azimut;
 
go(M1, -speed);
go(M2, speed);
azimut=read_azimut_mag();
while(read_azimut_mag() >= (azimut - angle)) usleep(10000);
go(M1, 0);
go(M2, 0);
}
 
void turnR(unsigned char angle, signed char speed) // otoci robota o zadany uhel (otaceni v pravo)
{
int azimut;
 
go(M1, speed);
go(M2, -speed);
azimut=read_azimut_mag();
198,10 → 262,6
go(M2, 0);
}
 
void turnR() // otoci robota o zadany uhel
{
}
 
int main(int argc, char *argv[], char *envp[])
{
int filtr;
211,10 → 271,11
 
i2c_init();
 
last_cross=0;
last_cross=-1;
command=0;
filtr=0;
done=0;
korekce=0;
 
pthread_create(&thread_1, NULL, print_tele, NULL);
pthread_create(&thread_3, NULL, gps, NULL);
288,28 → 349,73
case 'g':
usleep(180000); // simulace ostatnich cidel (zdrzeni)
 
//!!!KAKL FindNearestCrossing();
FindNearestCrossing();
//!!! FindWay();
 
vzdalenost=echo(US3_Addr);
usleep(30000);
lus=echo(US1_Addr);
usleep(30000);
rus=echo(US2_Addr);
usleep(30000);
 
if ((vzdalenost>60)&&(vzdalenost<80))
{
go(M1, 60);
go(M2, 60);
 
//FindNearestCrossing();
 
// if(korekce==0)
{
go(M1, 80);
go(M2, 80);
};
korekce=0;
filtr=0;
break;
};
if ((vzdalenost>30)&&(vzdalenost<130))
{
/*
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, 30);
go(M1, 40);
go(M2, 60);
}
else
{
go(M1, 60);
go(M2, 30);
go(M2, 40);
}
}
else
347,14 → 453,16
if (pom!='i') command=pom;
 
pRouraO = fopen("/home/ble/pipe","w");
fprintf(pRouraO,"US: %u cm - ",vzdalenost);
fprintf(pRouraO,"US3: %u cm - ",vzdalenost);
 
pthread_mutex_lock(&mutex);
fprintf(pRouraO,"%fN %fE - ", nord, east);
fprintf(pRouraO,"Bod:%d - ", last_cross);
fprintf(pRouraO,"Bod:%d - ", bod);
fprintf(pRouraO,"Len:%.1f m /", GeoCalc::EllipsoidDistance(nord, east, cros[last_cross].n, cros[last_cross].e));
fprintf(pRouraO,"Az:%.2f Deg - ", GeoCalc::GCAzimuth(nord, east, cros[last_cross].n, cros[last_cross].e));
fprintf(pRouraO,"AzMag: %d (0-255)\n", read_azimut_mag());
fprintf(pRouraO,"US1: %u cm - US2 %u cm\n",lus,rus);
 
pthread_mutex_unlock(&mutex);
 
fclose(pRouraO);
382,6 → 490,7
}
}
 
 
/*
void *sensors(void *unused)
{
389,20 → 498,12
 
while(true)
{
I2C_addr(US_Addr);
Buf[0]=0x0;
Buf[1]=0x51;
write(file, Buf, 2);
usleep(80000);
read(file, Buf, 3);
vzdalenost=(Buf[1]*256+Buf[2]);
usleep(300000);
 
I2C_addr(PIC_Addr);
Buf[0]=command;
write(file, Buf, 1);
read(file, Buf, 1);
ble=Buf[0];
vzdalenost=echo(US3_Addr);
usleep(20000);
left=echo(US1_Addr);
usleep(20000);
right=echo(US1_Addr);
usleep(20000);
}
}
*/