Subversion Repositories svnkaklik

Rev

Rev 372 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 372 Rev 377
Line 36... Line 36...
36 
 
 36 
 
37 
using namespace std;
 37 
using namespace std;
38 
 
 38 
 
39 
 
 39 
 
40 
#define BC_Addr	    0x0B
 40 
#define BC_Addr	    0x0B
- 
 
 41 
#define US1_Addr	(0xE2>>1)
- 
 
 42 
#define US2_Addr	(0xE4>>1)
41 
#define US3_Addr	0x70    // 0xE0 in fact; Sonar na doprovod
 43 
#define US3_Addr	(0xE6>>1)
42 
#define CMPS_Addr   0x60    // 0xC0
 44 
#define CMPS_Addr   (0xC0>>1)
43 
#define M1	0x50 // 0xA0 in fact
 45 
#define M1	        (0xA0>>1)
44 
#define M2	0x51 // 0xA2 in fact
 46 
#define M2	        (0xA2>>1)
45 
 
 47 
 
46 
#define SEVER 0
 48 
#define SEVER 122
47 
 
 49 
 
48 
char vystup[50];
 50 
char vystup[50];
49 
pthread_t thread_1, thread_2, thread_3;
 51 
pthread_t thread_1, thread_2, thread_3;
50 
FILE *pRouraO,*pRouraI;
 52 
FILE *pRouraO,*pRouraI;
51 
unsigned int vzdalenost;
 53 
unsigned int vzdalenost;
52 
char command,ble;
 54 
char command,ble;
- 
 
 55 
int param;
53 
int file;
 56 
int file;
54 
double n, e;
 57 
double nord, east;
55 
unsigned char azimut_mag;
 58 
int done; // vlajka, ze se neco udelalo
56 
 
 - 
 
- 
 
 59 
int last_cross; // posledni krizovatka
57 
 
 60 
 
58 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 61 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
59 
 
 62 
 
60 
void *print_tele(void *unused);
 63 
void *print_tele(void *unused);
61 
void *gps(void *unused);
 64 
void *gps(void *unused);
Line 90... Line 93...
90 
    usleep(80000);
 93 
    usleep(80000);
91 
    read(file, Buf, 3);
 94 
    read(file, Buf, 3);
92 
    return (Buf[1]*256+Buf[2]);
 95 
    return (Buf[1]*256+Buf[2]);
93 
}
 96 
}
94 
 
 97 
 
- 
 
 98 
unsigned char read_azimut_mag()  // precte azimut z kompasu
- 
 
 99 
{
- 
 
 100 
    char Buf[3];      // promena pro manipulaci s i2c
- 
 
 101 
 
- 
 
 102 
    I2C_addr(CMPS_Addr);
- 
 
 103 
    Buf[0]=1;
- 
 
 104 
    write(file,Buf,1);
- 
 
 105 
    read(file, Buf,1);
- 
 
 106 
    return (Buf[0]-SEVER);
- 
 
 107 
}
- 
 
 108 
 
- 
 
 109 
void calib()  // kalibrace kompasu
- 
 
 110 
{
- 
 
 111 
    char Buf[3];      // promena pro manipulaci s i2c
- 
 
 112 
 
- 
 
 113 
    I2C_addr(CMPS_Addr);
- 
 
 114 
    Buf[0]=15;
- 
 
 115 
    Buf[1]=0xFF;
- 
 
 116 
    write(file,Buf,2);
- 
 
 117 
}
- 
 
 118 
 
- 
 
 119 
void TL (unsigned char azimut2)
- 
 
 120 
{
- 
 
 121 
    unsigned char azimut1;
- 
 
 122 
 
- 
 
 123 
    go(M1, 0);
- 
 
 124 
    go(M2, 120);
- 
 
 125 
    do
- 
 
 126 
    {
- 
 
 127 
        azimut1=read_azimut_mag();
- 
 
 128 
//printf("az1: %d - az2: %d\n", azimut1, azimut2);
- 
 
 129 
        usleep(10000);
- 
 
 130 
    } while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2) );
- 
 
 131 
    go(M1, 0);
- 
 
 132 
    go(M2, 0);
- 
 
 133 
}
- 
 
 134 
 
- 
 
 135 
void TR (unsigned char azimut2)
- 
 
 136 
{
- 
 
 137 
    unsigned char azimut1;
- 
 
 138 
 
- 
 
 139 
    go(M1, 120);
- 
 
 140 
    go(M2, 0);
- 
 
 141 
    do
- 
 
 142 
    {
- 
 
 143 
        azimut1=read_azimut_mag();
- 
 
 144 
        usleep(10000);
- 
 
 145 
    } while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2) );
- 
 
 146 
    go(M1, 0);
- 
 
 147 
    go(M2, 0);
- 
 
 148 
}
- 
 
 149 
 
95 
void FindNearestCrossing(void)
 150 
void FindNearestCrossing(void)
96 
{
 151 
{
97 
    int n;
 152 
    int n;
- 
 
 153 
    double dist, pomN, pomE;
98 
 
 154 
 
- 
 
 155 
    pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
- 
 
 156 
    pomN=nord; pomE=east;
- 
 
 157 
    pthread_mutex_unlock(&mutex);
- 
 
 158 
    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
99 
    n=0;
 159 
    n=0;
100 
    do
 160 
    do
101 
    {
 161 
    {
102 
        if (0==cros[n].id) break;
 162 
        if (0==cros[n].id) break;
103 
        if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)
 163 
        dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e);
- 
 
 164 
        if (dist <= (double)cros[n].dia)
104 
        {
 165 
        {
- 
 
 166 
//printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
- 
 
 167 
            if (done==0)
- 
 
 168 
            {
- 
 
 169 
                last_cross=n;
105 
            #include "nav.h"
 170 
                #include "nav.h"
- 
 
 171 
                done=1;
- 
 
 172 
            }
106 
            break;
 173 
            break;
107 
        }
 174 
        }
108 
    } while(++n<POINTS);
 175 
    } while(++n<POINTS);
109 
}
 176 
}
110 
 
 177 
 
Line 117... Line 184...
117 
		return -1;
 184 
		return -1;
118 
	}
 185 
	}
119 
	return 0;
 186 
	return 0;
120 
}
 187 
}
121 
 
 188 
 
122 
int read_azimut_mag()  // precte azimut z kompasu
 - 
 
123 
{
 - 
 
124 
char Buf[3];      // promena pro manipulaci s i2c
 - 
 
125 
    I2C_addr(CMPS_Addr);
 - 
 
126 
    Buf[0]=1;
 - 
 
127 
    write(file,Buf,1);
 - 
 
128 
    read(file, Buf,1);
 - 
 
129 
    return Buf[0];
 - 
 
130 
}
 - 
 
131 
 
 - 
 
132 
void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel
 189 
void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel
133 
{
 190 
{
134 
int azimut;
 191 
int azimut;
135 
 
 192 
 
136 
    go(M1, speed);
 193 
    go(M1, speed);
137 
    go(M2, -speed);
 194 
    go(M2, -speed);
138 
    azimut=read_azimut_mag();
 195 
    azimut=read_azimut_mag();
139 
    while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
 196 
    while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
140 
    go(M1, 0);
 197 
    go(M1, 0);
141 
    go(M2, 0);
 198 
    go(M2, 0);
142 
    command=0;
 - 
 
143 
}
 199 
}
144 
 
 200 
 
145 
void turnR()    // otoci robota o zadany uhel
 201 
void turnR()    // otoci robota o zadany uhel
146 
{
 202 
{
147 
}
 203 
}
Line 153... Line 209...
153 
 
 209 
 
154 
	fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
 210 
	fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
155 
 
 211 
 
156 
    i2c_init();
 212 
    i2c_init();
157 
 
 213 
 
- 
 
 214 
    last_cross=0;
- 
 
 215 
    command=0;
- 
 
 216 
    filtr=0;
- 
 
 217 
    done=0;
- 
 
 218 
 
158 
    pthread_create(&thread_1, NULL, print_tele, NULL);
 219 
    pthread_create(&thread_1, NULL, print_tele, NULL);
159 
    pthread_create(&thread_3, NULL, gps, NULL);
 220 
    pthread_create(&thread_3, NULL, gps, NULL);
160 
//    pthread_create(&thread_2, NULL, sensors, NULL);
 221 
//    pthread_create(&thread_2, NULL, sensors, NULL);
161 
 
 222 
 
162 
    command=0;
 - 
 
163 
    filtr=0;
 - 
 
164 
 
 - 
 
165 
    while(true)
 223 
    while(true)
166 
    {
 224 
    {
167 
        switch (command)
 225 
        switch (command)
168 
        {
 226 
        {
169 
            case 'f':   // forward
 227 
            case 'f':   // forward
170 
                go(M1, 70);
 228 
                go(M1, param);
171 
                go(M2, 70);
 229 
                go(M2, param);
- 
 
 230 
                sleep(1);
172 
                command=0;
 231 
                command=0;
173 
                break;
 232 
                break;
- 
 
 233 
 
174 
            case 'b':   // backward
 234 
            case 'b':   // backward
175 
                go(M1, -70);
 235 
                go(M1, -param);
176 
                go(M2, -70);
 236 
                go(M2, -param);
- 
 
 237 
                sleep(1);
177 
                command=0;
 238 
                command=0;
178 
                break;
 239 
                break;
- 
 
 240 
 
179 
            case 'v':   // volnobeh
 241 
            case 'v':   // volnobeh
180 
                go(M1, -128);
 242 
                go(M1, -128);
181 
                go(M2, -128);
 243 
                go(M2, -128);
182 
                command=0;
 244 
                command=0;
183 
                break;
 245 
                break;
- 
 
 246 
 
184 
            case 's':   // stop
 247 
            case 's':   // stop
185 
                go(M1, 0);
 248 
                go(M1, 0);
186 
                go(M2, 0);
 249 
                go(M2, 0);
187 
                command=0;
 250 
                command=0;
188 
                break;
 251 
                break;
- 
 
 252 
 
189 
            case 'a':   // test otaceni
 253 
            case 'l':   // left
190 
                go(M1, 100);
 254 
                TL(param);
191 
                go(M2, -100);
 255 
                command=0;
- 
 
 256 
                break;
- 
 
 257 
 
- 
 
 258 
            case 'r':   // right
192 
                azimut_mag=read_azimut_mag();
 259 
                TR(param);
193 
                while(read_azimut_mag() >= (azimut_mag + 45)) usleep(10000);
 260 
                command=0;
194 
                go(M1, 0);
 261 
                break;
- 
 
 262 
 
- 
 
 263 
            case 'c':   // kalibrace kompasu
195 
                go(M2, 0);
 264 
                calib();
196 
                command=0;
 265 
                command=0;
197 
                break;
 266 
                break;
198 
 
 267 
 
199 
            case 't':   // test
 268 
            case 't':   // test
200 
                for(test=0;test<127;test++)
 269 
                for(test=0;test<127;test++)
Line 216... Line 285...
216 
                command=0;
 285 
                command=0;
217 
                break;
 286 
                break;
218 
 
 287 
 
219 
            case 'g':
 288 
            case 'g':
220 
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
 289 
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
221 
                azimut_mag=read_azimut_mag();
 - 
 
222 
 
 290 
 
223 
                FindNearestCrossing();
 291 
//!!!KAKL                FindNearestCrossing();
224 
 
 292 
 
225 
                vzdalenost=echo(US3_Addr);
 293 
                vzdalenost=echo(US3_Addr);
226 
                if ((vzdalenost>60)&&(vzdalenost<80))
 294 
                if ((vzdalenost>60)&&(vzdalenost<80))
227 
                {
 295 
                {
228 
                    go(M1, 60);
 296 
                    go(M1, 60);
Line 247... Line 315...
247 
                else
 315 
                else
248 
                {
 316 
                {
249 
                    filtr++;
 317 
                    filtr++;
250 
                    if (filtr>5)
 318 
                    if (filtr>5)
251 
                    {
 319 
                    {
252 
                        go(M1, 1);  // zastav, neni videt doprovod
 320 
                        go(M1, 0);  // zastav, neni videt doprovod
253 
                        go(M2, 1);
 321 
                        go(M2, 0);
254 
                        filtr=6;
 322 
                        filtr=6;
255 
                    }
 323 
                    }
256 
                };
 324 
                };
257 
                break;
 325 
                break;
258 
        }
 326 
        }
Line 267... Line 335...
267 
}
 335 
}
268 
 
 336 
 
269 
 
 337 
 
270 
void *print_tele(void *unused)
 338 
void *print_tele(void *unused)
271 
{
 339 
{
272 
    char string[2];
 340 
    char pom;
273 
 
 341 
 
274 
    while(true)
 342 
    while(true)
275 
    {
 343 
    {
276 
        pRouraI = fopen("/home/ble/pipe","r");
 344 
        pRouraI = fopen("/home/ble/pipe","r");
277 
        command=fgetc(pRouraI);
 345 
        fscanf(pRouraI,"%1s%d", &pom, &param);
278 
        string[0]=command;
 - 
 
279 
        string[1]=0;
 - 
 
280 
        fclose(pRouraI);
 346 
        fclose(pRouraI);
- 
 
 347 
        if (pom!='i') command=pom;
- 
 
 348 
 
281 
        pRouraO = fopen("/home/ble/pipe","w");
 349 
        pRouraO = fopen("/home/ble/pipe","w");
282 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
 350 
        fprintf(pRouraO,"US: %u cm - ",vzdalenost);
283 
 
 351 
 
284 
    pthread_mutex_lock(&mutex);
 352 
        pthread_mutex_lock(&mutex);
285 
   	fprintf(pRouraO,"%f N %f E\n", n, e);
 353 
        fprintf(pRouraO,"%fN %fE - ", nord, east);
- 
 
 354 
        fprintf(pRouraO,"Bod:%d - ", last_cross);
286 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
 355 
        fprintf(pRouraO,"Len:%.1f m /", GeoCalc::EllipsoidDistance(nord, east, cros[last_cross].n, cros[last_cross].e));
287 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
 356 
        fprintf(pRouraO,"Az:%.2f Deg - ", GeoCalc::GCAzimuth(nord, east, cros[last_cross].n, cros[last_cross].e));
288 
	fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag);
 357 
        fprintf(pRouraO,"AzMag: %d (0-255)\n", read_azimut_mag());
289 
    pthread_mutex_unlock(&mutex);
 358 
        pthread_mutex_unlock(&mutex);
290 
 
 359 
 
291 
        fclose(pRouraO);
 360 
        fclose(pRouraO);
292 
    }
 361 
    }
293 
}
 362 
}
294 
 
 363 
 
Line 305... Line 374...
305 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
 374 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
306 
        pomN=(N-nn*100)/60+nn;
 375 
        pomN=(N-nn*100)/60+nn;
307 
        ee=ldiv((long)E,100).quot;
 376 
        ee=ldiv((long)E,100).quot;
308 
        pomE=(E-ee*100)/60+ee;
 377 
        pomE=(E-ee*100)/60+ee;
309 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
 378 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
310 
        n=pomN; e=pomE;
 379 
        nord=pomN; east=pomE;
311 
        pthread_mutex_unlock(&mutex);
 380 
        pthread_mutex_unlock(&mutex);
312 
        usleep(800000);                     // NMEA nechodi castejc nez 1x za 1s
 381 
        usleep(800000);                     // NMEA nechodi castejc nez 1x za 1s
313 
    }
 382 
    }
314 
}
 383 
}
315 
 
 384