Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 368 → Rev 369

/roboti/Robotour/SW/vector/vector.cpp
32,6 → 32,7
#include <sys/stat.h>
#include <fcntl.h>
#include "geocalc.h"
#include "track.h"
 
using namespace std;
 
87,6 → 88,7
}
 
pthread_create(&thread_1, NULL, print_tele, NULL);
pthread_create(&thread_3, NULL, gps, NULL);
// pthread_create(&thread_2, NULL, sensors, NULL);
 
char Buf[64];
97,20 → 99,25
switch (command)
{
case 'f': // forward
go(M1, 20);
go(M2, 20);
go(M1, 70);
go(M2, 70);
command=0;
break;
case 'b': // backward
go(M1, -20);
go(M2, -20);
go(M1, -70);
go(M2, -70);
command=0;
break;
case 's': // stop
case 'v': // stop
go(M1, 0);
go(M2, 0);
command=0;
break;
case 's': // stop
go(M1, 1);
go(M2, 1);
command=0;
break;
case 'g':
usleep(180000);
I2C_addr(US_Addr);
151,6 → 158,7
close(file);
pthread_join(thread_1, NULL);
pthread_join(thread_2, NULL);
pthread_join(thread_3, NULL);
 
return 0;
}
172,8 → 180,8
 
pthread_mutex_lock(&mutex);
fprintf(pRouraO,"%f N %f E\n", n, e);
fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, 49.266667, 14.716667));
fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667));
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));
pthread_mutex_unlock(&mutex);
 
fclose(pRouraO);
183,17 → 191,21
void *gps(void *unused)
{
FILE *pRS232;
double pomN, pomE;
double N, E, pomN, pomE, nn, ee;
 
pRS232 = fopen("/dev/ttyS1","r");
 
while(true)
{
fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE);
pthread_mutex_lock(&mutex);
fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
nn=ldiv((long)N,100).quot; // prepocet DDMM.MM na DD.DD
pomN=(N-nn*100)/60+nn;
ee=ldiv((long)E,100).quot;
pomE=(E-ee*100)/60+ee;
pthread_mutex_lock(&mutex); // prepis souradnic do sdilenych promennych
n=pomN; e=pomE;
pthread_mutex_unlock(&mutex);
usleep(500000);
usleep(500000); // NMEA nechodi castejc nez 1x za 1s
}
}