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
*/