40,7 → 40,7 |
#define SRF02_SOFTWARE_REVISION 0x0 |
|
#define BC_Addr 0x0B |
#define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod |
#define US_Addr 0x70 // 0xE0 in fact |
#define M1 0x50 // 0xA0 in fact |
#define M2 0x51 // 0xA2 in fact |
|
76,39 → 76,8 |
write(file, Buf, 1); |
} |
|
unsigned int echo(int Addr) |
{ |
char Buf[3]; |
|
I2C_addr(Addr); |
Buf[0]=0x0; |
Buf[1]=0x51; |
write(file, Buf, 2); |
usleep(80000); |
read(file, Buf, 3); |
return (Buf[1]*256+Buf[2]); |
} |
|
void FindNearestCrossing(void) |
{ |
int n; |
|
n=0; |
do |
{ |
if (0==cros[n].id) break; |
if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia) |
{ |
#include "nav.h" |
break; |
} |
} while(++n<POINTS); |
} |
|
int main(int argc, char *argv[], char *envp[]) |
{ |
int filtr; |
|
fprintf(stdout, "**** Vector Control Programm ****\n"); |
|
file = open("/dev/i2c-0", O_RDWR); |
122,8 → 91,8 |
pthread_create(&thread_3, NULL, gps, NULL); |
// pthread_create(&thread_2, NULL, sensors, NULL); |
|
char Buf[64]; |
command=0; |
filtr=0; |
|
while(true) |
{ |
151,39 → 120,37 |
break; |
case 'g': |
usleep(180000); |
vzdalenost=echo(US3_Addr); |
if ((vzdalenost>60)&&(vzdalenost<80)) |
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]); |
if ((vzdalenost>50)&&(vzdalenost<80)) |
{ |
go(M1, 60); |
go(M2, 60); |
filtr=0; |
go(M1, 20); |
go(M2, 20); |
break; |
}; |
if ((vzdalenost>30)&&(vzdalenost<130)) |
if ((vzdalenost>30)&&(vzdalenost<120)) |
{ |
filtr=0; |
if (vzdalenost<55) |
if (vzdalenost<50) |
{ |
go(M1, 30); |
go(M2, 60); |
go(M1, 20); |
go(M2, 40); |
} |
else |
{ |
go(M1, 60); |
go(M2, 30); |
go(M1, 40); |
go(M2, 20); |
} |
} |
else |
{ |
filtr++; |
if (filtr>5) |
{ |
go(M1, 1); // zastav, neni videt doprovod |
go(M2, 1); |
filtr=6; |
} |
go(M1, 0); // zastav, neni videt doprovod |
go(M2, 0); |
}; |
FindNearestCrossing(); |
break; |
} |
}; |
226,11 → 193,11 |
FILE *pRS232; |
double N, E, pomN, pomE, nn, ee; |
|
pRS232 = fopen("/dev/ttyS1","r"); |
|
while(true) |
{ |
pRS232 = fopen("/dev/ttyS1","r"); |
fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA |
fclose(pRS232); |
nn=ldiv((long)N,100).quot; // prepocet DDMM.MM na DD.DD |
pomN=(N-nn*100)/60+nn; |
ee=ldiv((long)E,100).quot; |
238,7 → 205,7 |
pthread_mutex_lock(&mutex); // prepis souradnic do sdilenych promennych |
n=pomN; e=pomE; |
pthread_mutex_unlock(&mutex); |
usleep(800000); // NMEA nechodi castejc nez 1x za 1s |
usleep(500000); // NMEA nechodi castejc nez 1x za 1s |
} |
} |
|