Subversion Repositories svnkaklik

Compare Revisions

Regard whitespace Rev 377 → Rev 376

/roboti/Robotour/SW/vector/nav.h
File deleted
/roboti/Robotour/SW/vector/track.h
File deleted
/roboti/Robotour/SW/vector/vector.cpp
38,14 → 38,12
 
 
#define BC_Addr 0x0B
#define US1_Addr (0xE2>>1)
#define US2_Addr (0xE4>>1)
#define US3_Addr (0xE6>>1)
#define CMPS_Addr (0xC0>>1)
#define M1 (0xA0>>1)
#define M2 (0xA2>>1)
#define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod
#define CMPS_Addr 0x60 // 0xC0
#define M1 0x50 // 0xA0 in fact
#define M2 0x51 // 0xA2 in fact
 
#define SEVER 122
#define SEVER 0
 
char vystup[50];
pthread_t thread_1, thread_2, thread_3;
52,12 → 50,11
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
char command,ble;
int param;
int file;
double nord, east;
int done; // vlajka, ze se neco udelalo
int last_cross; // posledni krizovatka
double n, e;
unsigned char azimut_mag;
 
 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
void *print_tele(void *unused);
95,81 → 92,17
return (Buf[1]*256+Buf[2]);
}
 
unsigned char read_azimut_mag() // precte azimut z kompasu
{
char Buf[3]; // promena pro manipulaci s i2c
 
I2C_addr(CMPS_Addr);
Buf[0]=1;
write(file,Buf,1);
read(file, Buf,1);
return (Buf[0]-SEVER);
}
 
void calib() // kalibrace kompasu
{
char Buf[3]; // promena pro manipulaci s i2c
 
I2C_addr(CMPS_Addr);
Buf[0]=15;
Buf[1]=0xFF;
write(file,Buf,2);
}
 
void TL (unsigned char azimut2)
{
unsigned char azimut1;
 
go(M1, 0);
go(M2, 120);
do
{
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) );
go(M1, 0);
go(M2, 0);
}
 
void TR (unsigned char azimut2)
{
unsigned char azimut1;
 
go(M1, 120);
go(M2, 0);
do
{
azimut1=read_azimut_mag();
usleep(10000);
} while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2) );
go(M1, 0);
go(M2, 0);
}
 
void FindNearestCrossing(void)
{
int n;
double dist, pomN, pomE;
 
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
n=0;
do
{
if (0==cros[n].id) break;
dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e);
if (dist <= (double)cros[n].dia)
if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)
{
//printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
if (done==0)
{
last_cross=n;
#include "nav.h"
done=1;
}
break;
}
} while(++n<POINTS);
186,6 → 119,16
return 0;
}
 
int read_azimut_mag() // precte azimut z kompasu
{
char Buf[3]; // promena pro manipulaci s i2c
I2C_addr(CMPS_Addr);
Buf[0]=1;
write(file,Buf,1);
read(file, Buf,1);
return Buf[0];
}
 
void turnL(unsigned char angle, signed char speed) // otoci robota o zadany uhel
{
int azimut;
196,6 → 139,7
while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
go(M1, 0);
go(M2, 0);
command=0;
}
 
void turnR() // otoci robota o zadany uhel
211,60 → 155,47
 
i2c_init();
 
last_cross=0;
command=0;
filtr=0;
done=0;
 
pthread_create(&thread_1, NULL, print_tele, NULL);
pthread_create(&thread_3, NULL, gps, NULL);
// pthread_create(&thread_2, NULL, sensors, NULL);
 
command=0;
filtr=0;
 
while(true)
{
switch (command)
{
case 'f': // forward
go(M1, param);
go(M2, param);
sleep(1);
go(M1, 70);
go(M2, 70);
command=0;
break;
 
case 'b': // backward
go(M1, -param);
go(M2, -param);
sleep(1);
go(M1, -70);
go(M2, -70);
command=0;
break;
 
case 'v': // volnobeh
go(M1, -128);
go(M2, -128);
command=0;
break;
 
case 's': // stop
go(M1, 0);
go(M2, 0);
command=0;
break;
 
case 'l': // left
TL(param);
case 'a': // test otaceni
go(M1, 100);
go(M2, -100);
azimut_mag=read_azimut_mag();
while(read_azimut_mag() >= (azimut_mag + 45)) usleep(10000);
go(M1, 0);
go(M2, 0);
command=0;
break;
 
case 'r': // right
TR(param);
command=0;
break;
 
case 'c': // kalibrace kompasu
calib();
command=0;
break;
 
case 't': // test
for(test=0;test<127;test++)
{
287,8 → 218,9
 
case 'g':
usleep(180000); // simulace ostatnich cidel (zdrzeni)
azimut_mag=read_azimut_mag();
 
//!!!KAKL FindNearestCrossing();
FindNearestCrossing();
 
vzdalenost=echo(US3_Addr);
if ((vzdalenost>60)&&(vzdalenost<80))
317,8 → 249,8
filtr++;
if (filtr>5)
{
go(M1, 0); // zastav, neni videt doprovod
go(M2, 0);
go(M1, 1); // zastav, neni videt doprovod
go(M2, 1);
filtr=6;
}
};
337,24 → 269,23
 
void *print_tele(void *unused)
{
char pom;
char string[2];
 
while(true)
{
pRouraI = fopen("/home/ble/pipe","r");
fscanf(pRouraI,"%1s%d", &pom, &param);
command=fgetc(pRouraI);
string[0]=command;
string[1]=0;
fclose(pRouraI);
if (pom!='i') command=pom;
 
pRouraO = fopen("/home/ble/pipe","w");
fprintf(pRouraO,"US: %u cm - ",vzdalenost);
fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
 
pthread_mutex_lock(&mutex);
fprintf(pRouraO,"%fN %fE - ", nord, east);
fprintf(pRouraO,"Bod:%d - ", last_cross);
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,"%f N %f E\n", n, e);
fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag);
pthread_mutex_unlock(&mutex);
 
fclose(pRouraO);
376,7 → 307,7
ee=ldiv((long)E,100).quot;
pomE=(E-ee*100)/60+ee;
pthread_mutex_lock(&mutex); // prepis souradnic do sdilenych promennych
nord=pomN; east=pomE;
n=pomN; e=pomE;
pthread_mutex_unlock(&mutex);
usleep(800000); // NMEA nechodi castejc nez 1x za 1s
}