Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
365 kakl 1 
/*****************************************************************************/
2 
/*
3 
 *  vector.cpp - Control program for Vector robot
4 
 *
5 
 *      Copyright (C) 2007 KAKL
6 
 *
7 
 *      This program is free software; you can redistribute it and/or modify
8 
 *      it under the terms of the GNU General Public License as published by
9 
 *      the Free Software Foundation; either version 2 of the License, or
10 
 *      (at your option) any later version.
11 
 *
12 
 *      This program is distributed in the hope that it will be useful,
13 
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
14 
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 
 *      GNU General Public License for more details.
16 
 *
17 
 */
18 
/*****************************************************************************/
19 
 
20 
#include <iostream>
21 
#include <getopt.h>
22 
#include <errno.h>
23 
#include <string.h>
24 
#include <pthread.h>
25 
#include <stdio.h>
26 
#include <stdlib.h>
27 
#include <unistd.h>
28 
#include "linux/i2c-dev.h"
29 
#include "linux/i2c.h"
30 
#include <sys/ioctl.h>
31 
#include <sys/types.h>
32 
#include <sys/stat.h>
33 
#include <fcntl.h>
34 
#include "geocalc.h"
369 kakl 35 
#include "track.h"
365 kakl 36 
 
37 
using namespace std;
38 
 
39 
 
40 
#define BC_Addr	    0x0B
377 kakl 41 
#define US1_Addr	(0xE2>>1)
42 
#define US2_Addr	(0xE4>>1)
43 
#define US3_Addr	(0xE6>>1)
44 
#define CMPS_Addr   (0xC0>>1)
45 
#define M1	        (0xA0>>1)
46 
#define M2	        (0xA2>>1)
365 kakl 47 
 
377 kakl 48 
#define SEVER 122
372 kakl 49 
 
367 kakl 50 
char vystup[50];
51 
pthread_t thread_1, thread_2, thread_3;
365 kakl 52 
FILE *pRouraO,*pRouraI;
379 kakl 53 
unsigned int vzdalenost, lus, rus;
365 kakl 54 
char command,ble;
377 kakl 55 
int param;
365 kakl 56 
int file;
377 kakl 57 
double nord, east;
58 
int done; // vlajka, ze se neco udelalo
59 
int last_cross; // posledni krizovatka
379 kakl 60 
int korekce; // Byla korekce podle kompasu nebo zdi
61 
int bod;
365 kakl 62 
 
367 kakl 63 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
64 
 
365 kakl 65 
void *print_tele(void *unused);
367 kakl 66 
void *gps(void *unused);
366 kakl 67 
//void *sensors(void *unused);
365 kakl 68 
 
379 kakl 69 
void I2C_addr (int Addr)  // vybere adresu cidla se kterym se bude komunikovat
365 kakl 70 
{
71 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
72 
    {
73 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
74 
        exit(-5);
75 
    }
76 
}
77 
 
379 kakl 78 
int i2c_init()   // zinicializuje i2c
79 
{
80 
	file = open("/dev/i2c-0", O_RDWR);
81 
	if (file < 0)
82 
	{
83 
		cerr << "Could not open /dev/i2c-0." << endl;
84 
		return -1;
85 
	}
86 
	return 0;
87 
}
88 
 
372 kakl 89 
void go (int Addr, int speed) // nastavi rychlost motoru
367 kakl 90 
{
91 
    char Buf[1];
92 
 
93 
    I2C_addr (Addr);
94 
    Buf[0]=speed;
95 
    write(file, Buf, 1);
96 
}
97 
 
372 kakl 98 
unsigned int echo(int Addr)  // precte vzdalenost z US cidla
371 kakl 99 
{
100 
    char Buf[3];
101 
 
102 
    I2C_addr(Addr);
103 
    Buf[0]=0x0;
104 
    Buf[1]=0x51;
105 
    write(file, Buf, 2);
106 
    usleep(80000);
107 
    read(file, Buf, 3);
108 
    return (Buf[1]*256+Buf[2]);
109 
}
110 
 
377 kakl 111 
unsigned char read_azimut_mag()  // precte azimut z kompasu
112 
{
113 
    char Buf[3];      // promena pro manipulaci s i2c
114 
 
115 
    I2C_addr(CMPS_Addr);
116 
    Buf[0]=1;
117 
    write(file,Buf,1);
118 
    read(file, Buf,1);
119 
    return (Buf[0]-SEVER);
120 
}
121 
 
122 
void calib()  // kalibrace kompasu
123 
{
124 
    char Buf[3];      // promena pro manipulaci s i2c
125 
 
126 
    I2C_addr(CMPS_Addr);
127 
    Buf[0]=15;
128 
    Buf[1]=0xFF;
129 
    write(file,Buf,2);
130 
}
131 
 
379 kakl 132 
void TL (unsigned char azimut2) // otoci robota na zadany azimut rotaci vlevo
377 kakl 133 
{
134 
    unsigned char azimut1;
135 
 
136 
    go(M1, 0);
137 
    go(M2, 120);
138 
    do
139 
    {
140 
        azimut1=read_azimut_mag();
141 
//printf("az1: %d - az2: %d\n", azimut1, azimut2);
142 
        usleep(10000);
379 kakl 143 
    } while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2)&&((azimut1+4)!=azimut2) );
377 kakl 144 
    go(M1, 0);
145 
    go(M2, 0);
146 
}
147 
 
379 kakl 148 
void TR (unsigned char azimut2) // otoci robota na zadany azimut rotaci v pravo
377 kakl 149 
{
150 
    unsigned char azimut1;
151 
 
152 
    go(M1, 120);
153 
    go(M2, 0);
154 
    do
155 
    {
156 
        azimut1=read_azimut_mag();
157 
        usleep(10000);
379 kakl 158 
    } while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2)&&((azimut1-4)!=azimut2) );
377 kakl 159 
    go(M1, 0);
160 
    go(M2, 0);
161 
}
162 
 
379 kakl 163 
void SlowLeft()
371 kakl 164 
{
379 kakl 165 
    go(M1, 40);
166 
    go(M2, 60);
167 
}
168 
 
169 
void SlowRight()
170 
{
171 
    go(M1, 60);
172 
    go(M2, 40);
173 
}
174 
 
175 
void FindNearestCrossing(void) // vykonej nejakou akci (zatoceni) pokud se robot nachazi pobliz nejakeho waypointu
176 
{
371 kakl 177 
    int n;
377 kakl 178 
    double dist, pomN, pomE;
379 kakl 179 
    unsigned char azimut;
371 kakl 180 
 
377 kakl 181 
    pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
182 
    pomN=nord; pomE=east;
183 
    pthread_mutex_unlock(&mutex);
379 kakl 184 
//    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
371 kakl 185 
    n=0;
186 
    do
187 
    {
188 
        if (0==cros[n].id) break;
377 kakl 189 
        dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e);
190 
        if (dist <= (double)cros[n].dia)
371 kakl 191 
        {
377 kakl 192 
//printf("Point ID: %d - Distance: %f\n",cros[n].id, dist);
379 kakl 193 
            if (last_cross!=cros[n].id)
377 kakl 194 
            {
379 kakl 195 
                bod=cros[n].id;
377 kakl 196 
                #include "nav.h"
379 kakl 197 
//                done=1;
377 kakl 198 
            }
371 kakl 199 
            break;
200 
        }
201 
    } while(++n<POINTS);
202 
}
203 
 
379 kakl 204 
/*
205 
void FindWay(void) // koriguj smer podle kompasu
365 kakl 206 
{
379 kakl 207 
    int n;
208 
    double dist, pomN, pomE;
209 
 
210 
    pthread_mutex_lock(&mutex);         // prepis souradnic do pracovnich promennych
211 
    pomN=nord; pomE=east;
212 
    pthread_mutex_unlock(&mutex);
213 
//    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
214 
    n=0;
215 
    do
216 
    {
217 
        if (0==way[n].id) break;
218 
        dist=GeoCalc::EllipsoidDistance(pomN, pomE, way[n].n, way[n].e);
219 
        if (dist <= (double)way[n].dia)
220 
        {
221 
            if (last_way!=way[n].id)
222 
            {
223 
                last_way=way[n].id;
224 
                switch (way[n].id)
225 
                {
226 
                    case 1:
227 
                        TL(way[n].azimut);
228 
                        break;
229 
                    case 2:
230 
                        TL(80);
231 
                        break;
232 
                };
233 
 
234 
            }
235 
            break;
236 
        }
237 
    } while(++n<POINTS);
372 kakl 238 
}
379 kakl 239 
*/
365 kakl 240 
 
379 kakl 241 
void turnL(unsigned char angle, signed char speed)    // otoci robota o zadany uhel (otaceni v levo)
372 kakl 242 
{
243 
int azimut;
244 
 
379 kakl 245 
    go(M1, -speed);
246 
    go(M2, speed);
247 
    azimut=read_azimut_mag();
248 
    while(read_azimut_mag() >= (azimut - angle)) usleep(10000);
249 
    go(M1, 0);
250 
    go(M2, 0);
251 
}
252 
 
253 
void turnR(unsigned char angle, signed char speed)    // otoci robota o zadany uhel (otaceni v pravo)
254 
{
255 
int azimut;
256 
 
372 kakl 257 
    go(M1, speed);
258 
    go(M2, -speed);
259 
    azimut=read_azimut_mag();
260 
    while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
261 
    go(M1, 0);
262 
    go(M2, 0);
263 
}
264 
 
265 
int main(int argc, char *argv[], char *envp[])
266 
{
267 
    int filtr;
268 
    signed char test;
269 
 
270 
	fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
271 
 
272 
    i2c_init();
273 
 
379 kakl 274 
    last_cross=-1;
377 kakl 275 
    command=0;
276 
    filtr=0;
277 
    done=0;
379 kakl 278 
    korekce=0;
377 kakl 279 
 
365 kakl 280 
    pthread_create(&thread_1, NULL, print_tele, NULL);
369 kakl 281 
    pthread_create(&thread_3, NULL, gps, NULL);
365 kakl 282 
//    pthread_create(&thread_2, NULL, sensors, NULL);
283 
 
284 
    while(true)
285 
    {
367 kakl 286 
        switch (command)
366 kakl 287 
        {
367 kakl 288 
            case 'f':   // forward
377 kakl 289 
                go(M1, param);
290 
                go(M2, param);
291 
                sleep(1);
367 kakl 292 
                command=0;
293 
                break;
377 kakl 294 
 
367 kakl 295 
            case 'b':   // backward
377 kakl 296 
                go(M1, -param);
297 
                go(M2, -param);
298 
                sleep(1);
367 kakl 299 
                command=0;
300 
                break;
377 kakl 301 
 
372 kakl 302 
            case 'v':   // volnobeh
303 
                go(M1, -128);
304 
                go(M2, -128);
305 
                command=0;
306 
                break;
377 kakl 307 
 
372 kakl 308 
            case 's':   // stop
367 kakl 309 
                go(M1, 0);
310 
                go(M2, 0);
311 
                command=0;
312 
                break;
377 kakl 313 
 
314 
            case 'l':   // left
315 
                TL(param);
369 kakl 316 
                command=0;
317 
                break;
372 kakl 318 
 
377 kakl 319 
            case 'r':   // right
320 
                TR(param);
321 
                command=0;
322 
                break;
323 
 
324 
            case 'c':   // kalibrace kompasu
325 
                calib();
326 
                command=0;
327 
                break;
328 
 
372 kakl 329 
            case 't':   // test
330 
                for(test=0;test<127;test++)
331 
                {
332 
                    go(M1, test);
333 
                    go(M2, test);
334 
                    usleep(10000);
335 
                };
336 
                go(M1, 127);
337 
                go(M2, 127);
338 
                for(test=127;test>-128;test--)
339 
                {
340 
                    go(M1, test);
341 
                    go(M2, test);
342 
                    usleep(10000);
343 
                };
344 
                go(M1, -128);
345 
                go(M2, -128);
346 
                command=0;
347 
                break;
348 
 
367 kakl 349 
            case 'g':
372 kakl 350 
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
351 
 
379 kakl 352 
                FindNearestCrossing();
353 
//!!!                FindWay();
372 kakl 354 
 
371 kakl 355 
                vzdalenost=echo(US3_Addr);
379 kakl 356 
                usleep(30000);
357 
                lus=echo(US1_Addr);
358 
                usleep(30000);
359 
                rus=echo(US2_Addr);
360 
                usleep(30000);
361 
 
371 kakl 362 
                if ((vzdalenost>60)&&(vzdalenost<80))
367 kakl 363 
                {
379 kakl 364 
 
365 
//FindNearestCrossing();
366 
 
367 
//                    if(korekce==0)
368 
                    {
369 
                        go(M1, 80);
370 
                        go(M2, 80);
371 
                    };
372 
                    korekce=0;
371 kakl 373 
                    filtr=0;
367 kakl 374 
                    break;
375 
                };
371 kakl 376 
                if ((vzdalenost>30)&&(vzdalenost<130))
367 kakl 377 
                {
379 kakl 378 
/*
379 
                    if ((lus<70)&&(lus>30)&&(vzdalenost>60)) // prekazka vlevo
380 
                    {
381 
                        go(M1, 60);
382 
                        go(M2, 40);
383 
                        break;
384 
                    };
385 
 
386 
*/
387 
/*
388 
                    if ((rus<40)&&(rus>30)) // prekazka vpravo
389 
                    {
390 
                        go(M1, 40);
391 
                        go(M2, 60);
392 
                    };
393 
*/
394 
 
371 kakl 395 
                    filtr=0;
379 kakl 396 
/*
397 
                    if (vzdalenost<40)
398 
                    {
399 
                        go(M1, 40);
400 
                        go(M2, 60);
401 
                        break;
402 
                    }
403 
                    if (vzdalenost>100)
404 
                    {
405 
                        go(M1, 60);
406 
                        go(M2, 40);
407 
                        break;
408 
                    }
409 
*/
371 kakl 410 
                    if (vzdalenost<55)
367 kakl 411 
                    {
379 kakl 412 
                        go(M1, 40);
371 kakl 413 
                        go(M2, 60);
367 kakl 414 
                    }
415 
                    else
416 
                    {
371 kakl 417 
                        go(M1, 60);
379 kakl 418 
                        go(M2, 40);
367 kakl 419 
                    }
420 
                }
421 
                else
422 
                {
371 kakl 423 
                    filtr++;
424 
                    if (filtr>5)
425 
                    {
377 kakl 426 
                        go(M1, 0);  // zastav, neni videt doprovod
427 
                        go(M2, 0);
371 kakl 428 
                        filtr=6;
429 
                    }
367 kakl 430 
                };
431 
                break;
366 kakl 432 
        }
365 kakl 433 
    };
366 kakl 434 
 
365 kakl 435 
	close(file);
436 
    pthread_join(thread_1, NULL);
437 
    pthread_join(thread_2, NULL);
369 kakl 438 
    pthread_join(thread_3, NULL);
365 kakl 439 
 
440 
	return 0;
441 
}
442 
 
366 kakl 443 
 
365 kakl 444 
void *print_tele(void *unused)
445 
{
377 kakl 446 
    char pom;
366 kakl 447 
 
365 kakl 448 
    while(true)
449 
    {
450 
        pRouraI = fopen("/home/ble/pipe","r");
377 kakl 451 
        fscanf(pRouraI,"%1s%d", &pom, &param);
365 kakl 452 
        fclose(pRouraI);
377 kakl 453 
        if (pom!='i') command=pom;
454 
 
365 kakl 455 
        pRouraO = fopen("/home/ble/pipe","w");
379 kakl 456 
        fprintf(pRouraO,"US3: %u cm - ",vzdalenost);
367 kakl 457 
 
377 kakl 458 
        pthread_mutex_lock(&mutex);
459 
        fprintf(pRouraO,"%fN %fE - ", nord, east);
379 kakl 460 
        fprintf(pRouraO,"Bod:%d - ", bod);
377 kakl 461 
        fprintf(pRouraO,"Len:%.1f m /", GeoCalc::EllipsoidDistance(nord, east, cros[last_cross].n, cros[last_cross].e));
462 
        fprintf(pRouraO,"Az:%.2f Deg - ", GeoCalc::GCAzimuth(nord, east, cros[last_cross].n, cros[last_cross].e));
463 
        fprintf(pRouraO,"AzMag: %d (0-255)\n", read_azimut_mag());
379 kakl 464 
        fprintf(pRouraO,"US1: %u cm - US2 %u cm\n",lus,rus);
465 
 
377 kakl 466 
        pthread_mutex_unlock(&mutex);
367 kakl 467 
 
365 kakl 468 
        fclose(pRouraO);
469 
    }
470 
}
471 
 
367 kakl 472 
void *gps(void *unused)
473 
{
474 
    FILE *pRS232;
369 kakl 475 
    double N, E, pomN, pomE, nn, ee;
367 kakl 476 
 
477 
    while(true)
478 
    {
371 kakl 479 
        pRS232 = fopen("/dev/ttyS1","r");
369 kakl 480 
        fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
371 kakl 481 
        fclose(pRS232);
369 kakl 482 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
483 
        pomN=(N-nn*100)/60+nn;
484 
        ee=ldiv((long)E,100).quot;
485 
        pomE=(E-ee*100)/60+ee;
486 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
377 kakl 487 
        nord=pomN; east=pomE;
367 kakl 488 
        pthread_mutex_unlock(&mutex);
371 kakl 489 
        usleep(800000);                     // NMEA nechodi castejc nez 1x za 1s
367 kakl 490 
    }
491 
}
492 
 
379 kakl 493 
 
366 kakl 494 
/*
365 kakl 495 
void *sensors(void *unused)
496 
{
497 
	char Buf[64];
498 
 
499 
    while(true)
500 
    {
379 kakl 501 
        vzdalenost=echo(US3_Addr);
502 
        usleep(20000);
503 
        left=echo(US1_Addr);
504 
        usleep(20000);
505 
        right=echo(US1_Addr);
506 
        usleep(20000);
365 kakl 507 
    }
508 
}
366 kakl 509 
*/