Subversion Repositories svnkaklik

Rev

Rev 371 | Go to most recent revision | Details | Compare with Previous | 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
372 kakl 41 
#define US3_Addr	0x70    // 0xE0 in fact; Sonar na doprovod
42 
#define CMPS_Addr   0x60    // 0xC0
367 kakl 43 
#define M1	0x50 // 0xA0 in fact
44 
#define M2	0x51 // 0xA2 in fact
365 kakl 45 
 
372 kakl 46 
#define SEVER 0
47 
 
367 kakl 48 
char vystup[50];
49 
pthread_t thread_1, thread_2, thread_3;
365 kakl 50 
FILE *pRouraO,*pRouraI;
51 
unsigned int vzdalenost;
52 
char command,ble;
53 
int file;
367 kakl 54 
double n, e;
372 kakl 55 
unsigned char azimut_mag;
365 kakl 56 
 
372 kakl 57 
 
367 kakl 58 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
59 
 
365 kakl 60 
void *print_tele(void *unused);
367 kakl 61 
void *gps(void *unused);
366 kakl 62 
//void *sensors(void *unused);
365 kakl 63 
 
64 
void I2C_addr (int Addr)
65 
{
66 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
67 
    {
68 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
69 
        exit(-5);
70 
    }
71 
}
72 
 
372 kakl 73 
void go (int Addr, int speed) // nastavi rychlost motoru
367 kakl 74 
{
75 
    char Buf[1];
76 
 
77 
    I2C_addr (Addr);
78 
    Buf[0]=speed;
79 
    write(file, Buf, 1);
80 
}
81 
 
372 kakl 82 
unsigned int echo(int Addr)  // precte vzdalenost z US cidla
371 kakl 83 
{
84 
    char Buf[3];
85 
 
86 
    I2C_addr(Addr);
87 
    Buf[0]=0x0;
88 
    Buf[1]=0x51;
89 
    write(file, Buf, 2);
90 
    usleep(80000);
91 
    read(file, Buf, 3);
92 
    return (Buf[1]*256+Buf[2]);
93 
}
94 
 
95 
void FindNearestCrossing(void)
96 
{
97 
    int n;
98 
 
99 
    n=0;
100 
    do
101 
    {
102 
        if (0==cros[n].id) break;
103 
        if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)
104 
        {
105 
            #include "nav.h"
106 
            break;
107 
        }
108 
    } while(++n<POINTS);
109 
}
110 
 
372 kakl 111 
int i2c_init()   // zinicializuje i2c
365 kakl 112 
{
113 
	file = open("/dev/i2c-0", O_RDWR);
114 
	if (file < 0)
115 
	{
116 
		cerr << "Could not open /dev/i2c-0." << endl;
117 
		return -1;
118 
	}
372 kakl 119 
	return 0;
120 
}
365 kakl 121 
 
372 kakl 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
133 
{
134 
int azimut;
135 
 
136 
    go(M1, speed);
137 
    go(M2, -speed);
138 
    azimut=read_azimut_mag();
139 
    while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
140 
    go(M1, 0);
141 
    go(M2, 0);
142 
    command=0;
143 
}
144 
 
145 
void turnR()    // otoci robota o zadany uhel
146 
{
147 
}
148 
 
149 
int main(int argc, char *argv[], char *envp[])
150 
{
151 
    int filtr;
152 
    signed char test;
153 
 
154 
	fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
155 
 
156 
    i2c_init();
157 
 
365 kakl 158 
    pthread_create(&thread_1, NULL, print_tele, NULL);
369 kakl 159 
    pthread_create(&thread_3, NULL, gps, NULL);
365 kakl 160 
//    pthread_create(&thread_2, NULL, sensors, NULL);
161 
 
367 kakl 162 
    command=0;
371 kakl 163 
    filtr=0;
366 kakl 164 
 
365 kakl 165 
    while(true)
166 
    {
367 kakl 167 
        switch (command)
366 kakl 168 
        {
367 kakl 169 
            case 'f':   // forward
369 kakl 170 
                go(M1, 70);
171 
                go(M2, 70);
367 kakl 172 
                command=0;
173 
                break;
174 
            case 'b':   // backward
369 kakl 175 
                go(M1, -70);
176 
                go(M2, -70);
367 kakl 177 
                command=0;
178 
                break;
372 kakl 179 
            case 'v':   // volnobeh
180 
                go(M1, -128);
181 
                go(M2, -128);
182 
                command=0;
183 
                break;
184 
            case 's':   // stop
367 kakl 185 
                go(M1, 0);
186 
                go(M2, 0);
187 
                command=0;
188 
                break;
372 kakl 189 
            case 'a':   // test otaceni
190 
                go(M1, 100);
191 
                go(M2, -100);
192 
                azimut_mag=read_azimut_mag();
193 
                while(read_azimut_mag() >= (azimut_mag + 45)) usleep(10000);
194 
                go(M1, 0);
195 
                go(M2, 0);
369 kakl 196 
                command=0;
197 
                break;
372 kakl 198 
 
199 
            case 't':   // test
200 
                for(test=0;test<127;test++)
201 
                {
202 
                    go(M1, test);
203 
                    go(M2, test);
204 
                    usleep(10000);
205 
                };
206 
                go(M1, 127);
207 
                go(M2, 127);
208 
                for(test=127;test>-128;test--)
209 
                {
210 
                    go(M1, test);
211 
                    go(M2, test);
212 
                    usleep(10000);
213 
                };
214 
                go(M1, -128);
215 
                go(M2, -128);
216 
                command=0;
217 
                break;
218 
 
367 kakl 219 
            case 'g':
372 kakl 220 
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
221 
                azimut_mag=read_azimut_mag();
222 
 
223 
                FindNearestCrossing();
224 
 
371 kakl 225 
                vzdalenost=echo(US3_Addr);
226 
                if ((vzdalenost>60)&&(vzdalenost<80))
367 kakl 227 
                {
371 kakl 228 
                    go(M1, 60);
229 
                    go(M2, 60);
230 
                    filtr=0;
367 kakl 231 
                    break;
232 
                };
371 kakl 233 
                if ((vzdalenost>30)&&(vzdalenost<130))
367 kakl 234 
                {
371 kakl 235 
                    filtr=0;
236 
                    if (vzdalenost<55)
367 kakl 237 
                    {
371 kakl 238 
                        go(M1, 30);
239 
                        go(M2, 60);
367 kakl 240 
                    }
241 
                    else
242 
                    {
371 kakl 243 
                        go(M1, 60);
244 
                        go(M2, 30);
367 kakl 245 
                    }
246 
                }
247 
                else
248 
                {
371 kakl 249 
                    filtr++;
250 
                    if (filtr>5)
251 
                    {
252 
                        go(M1, 1);  // zastav, neni videt doprovod
253 
                        go(M2, 1);
254 
                        filtr=6;
255 
                    }
367 kakl 256 
                };
257 
                break;
366 kakl 258 
        }
365 kakl 259 
    };
366 kakl 260 
 
365 kakl 261 
	close(file);
262 
    pthread_join(thread_1, NULL);
263 
    pthread_join(thread_2, NULL);
369 kakl 264 
    pthread_join(thread_3, NULL);
365 kakl 265 
 
266 
	return 0;
267 
}
268 
 
366 kakl 269 
 
365 kakl 270 
void *print_tele(void *unused)
271 
{
367 kakl 272 
    char string[2];
366 kakl 273 
 
365 kakl 274 
    while(true)
275 
    {
276 
        pRouraI = fopen("/home/ble/pipe","r");
367 kakl 277 
        command=fgetc(pRouraI);
278 
        string[0]=command;
279 
        string[1]=0;
365 kakl 280 
        fclose(pRouraI);
281 
        pRouraO = fopen("/home/ble/pipe","w");
367 kakl 282 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
283 
 
284 
    pthread_mutex_lock(&mutex);
285 
   	fprintf(pRouraO,"%f N %f E\n", n, e);
369 kakl 286 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
287 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
372 kakl 288 
	fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag);
367 kakl 289 
    pthread_mutex_unlock(&mutex);
290 
 
365 kakl 291 
        fclose(pRouraO);
292 
    }
293 
}
294 
 
367 kakl 295 
void *gps(void *unused)
296 
{
297 
    FILE *pRS232;
369 kakl 298 
    double N, E, pomN, pomE, nn, ee;
367 kakl 299 
 
300 
    while(true)
301 
    {
371 kakl 302 
        pRS232 = fopen("/dev/ttyS1","r");
369 kakl 303 
        fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
371 kakl 304 
        fclose(pRS232);
369 kakl 305 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
306 
        pomN=(N-nn*100)/60+nn;
307 
        ee=ldiv((long)E,100).quot;
308 
        pomE=(E-ee*100)/60+ee;
309 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
367 kakl 310 
        n=pomN; e=pomE;
311 
        pthread_mutex_unlock(&mutex);
371 kakl 312 
        usleep(800000);                     // NMEA nechodi castejc nez 1x za 1s
367 kakl 313 
    }
314 
}
315 
 
366 kakl 316 
/*
365 kakl 317 
void *sensors(void *unused)
318 
{
319 
	char Buf[64];
320 
 
321 
    while(true)
322 
    {
323 
        I2C_addr(US_Addr);
324 
        Buf[0]=0x0;
325 
        Buf[1]=0x51;
326 
        write(file, Buf, 2);
327 
        usleep(80000);
328 
        read(file, Buf, 3);
329 
        vzdalenost=(Buf[1]*256+Buf[2]);
330 
        usleep(300000);
331 
 
332 
        I2C_addr(PIC_Addr);
333 
        Buf[0]=command;
334 
        write(file, Buf, 1);
335 
        read(file, Buf, 1);
336 
        ble=Buf[0];
337 
    }
338 
}
366 kakl 339 
*/