Subversion Repositories svnkaklik

Rev

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

Rev 369 Rev 371
Line 38... Line 38...
38 
 
 38 
 
39 
#define CMPS03_SOFTWARE_REVISION 0x0
 39 
#define CMPS03_SOFTWARE_REVISION 0x0
40 
#define SRF02_SOFTWARE_REVISION 0x0
 40 
#define SRF02_SOFTWARE_REVISION 0x0
41 
 
 41 
 
42 
#define BC_Addr	    0x0B
 42 
#define BC_Addr	    0x0B
43 
#define US_Addr	    0x70 // 0xE0 in fact
 43 
#define US3_Addr	    0x70 // 0xE0 in fact; Sonar na doprovod
44 
#define M1	0x50 // 0xA0 in fact
 44 
#define M1	0x50 // 0xA0 in fact
45 
#define M2	0x51 // 0xA2 in fact
 45 
#define M2	0x51 // 0xA2 in fact
46 
 
 46 
 
47 
char vystup[50];
 47 
char vystup[50];
48 
pthread_t thread_1, thread_2, thread_3;
 48 
pthread_t thread_1, thread_2, thread_3;
Line 74... Line 74...
74 
    I2C_addr (Addr);
 74 
    I2C_addr (Addr);
75 
    Buf[0]=speed;
 75 
    Buf[0]=speed;
76 
    write(file, Buf, 1);
 76 
    write(file, Buf, 1);
77 
}
 77 
}
78 
 
 78 
 
- 
 
 79 
unsigned int echo(int Addr)
- 
 
 80 
{
- 
 
 81 
    char Buf[3];
- 
 
 82 
 
- 
 
 83 
    I2C_addr(Addr);
- 
 
 84 
    Buf[0]=0x0;
- 
 
 85 
    Buf[1]=0x51;
- 
 
 86 
    write(file, Buf, 2);
- 
 
 87 
    usleep(80000);
- 
 
 88 
    read(file, Buf, 3);
- 
 
 89 
    return (Buf[1]*256+Buf[2]);
- 
 
 90 
}
- 
 
 91 
 
- 
 
 92 
void FindNearestCrossing(void)
- 
 
 93 
{
- 
 
 94 
    int n;
- 
 
 95 
 
- 
 
 96 
    n=0;
- 
 
 97 
    do
- 
 
 98 
    {
- 
 
 99 
        if (0==cros[n].id) break;
- 
 
 100 
        if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)
- 
 
 101 
        {
- 
 
 102 
            #include "nav.h"
- 
 
 103 
            break;
- 
 
 104 
        }
- 
 
 105 
    } while(++n<POINTS);
- 
 
 106 
}
- 
 
 107 
 
79 
int main(int argc, char *argv[], char *envp[])
 108 
int main(int argc, char *argv[], char *envp[])
80 
{
 109 
{
- 
 
 110 
    int filtr;
- 
 
 111 
 
81 
	fprintf(stdout, "**** Vector Control Programm ****\n");
 112 
	fprintf(stdout, "**** Vector Control Programm ****\n");
82 
 
 113 
 
83 
	file = open("/dev/i2c-0", O_RDWR);
 114 
	file = open("/dev/i2c-0", O_RDWR);
84 
	if (file < 0)
 115 
	if (file < 0)
85 
	{
 116 
	{
Line 89... Line 120...
89 
 
 120 
 
90 
    pthread_create(&thread_1, NULL, print_tele, NULL);
 121 
    pthread_create(&thread_1, NULL, print_tele, NULL);
91 
    pthread_create(&thread_3, NULL, gps, NULL);
 122 
    pthread_create(&thread_3, NULL, gps, NULL);
92 
//    pthread_create(&thread_2, NULL, sensors, NULL);
 123 
//    pthread_create(&thread_2, NULL, sensors, NULL);
93 
 
 124 
 
94 
    char Buf[64];
 - 
 
95 
    command=0;
 125 
    command=0;
- 
 
 126 
    filtr=0;
96 
 
 127 
 
97 
    while(true)
 128 
    while(true)
98 
    {
 129 
    {
99 
        switch (command)
 130 
        switch (command)
100 
        {
 131 
        {
Line 118... Line 149...
118 
                go(M2, 1);
 149 
                go(M2, 1);
119 
                command=0;
 150 
                command=0;
120 
                break;
 151 
                break;
121 
            case 'g':
 152 
            case 'g':
122 
usleep(180000);
 153 
usleep(180000);
123 
                I2C_addr(US_Addr);
 - 
 
124 
                Buf[0]=0x0;
 - 
 
125 
                Buf[1]=0x51;
 - 
 
126 
                write(file, Buf, 2);
 - 
 
127 
                usleep(80000);
 - 
 
128 
                read(file, Buf, 3);
 - 
 
129 
                vzdalenost=(Buf[1]*256+Buf[2]);
 154 
                vzdalenost=echo(US3_Addr);
130 
                if ((vzdalenost>50)&&(vzdalenost<80))
 155 
                if ((vzdalenost>60)&&(vzdalenost<80))
131 
                {
 156 
                {
132 
                    go(M1, 20);
 157 
                    go(M1, 60);
133 
                    go(M2, 20);
 158 
                    go(M2, 60);
- 
 
 159 
                    filtr=0;
134 
                    break;
 160 
                    break;
135 
                };
 161 
                };
136 
                if ((vzdalenost>30)&&(vzdalenost<120))
 162 
                if ((vzdalenost>30)&&(vzdalenost<130))
137 
                {
 163 
                {
- 
 
 164 
                    filtr=0;
138 
                    if (vzdalenost<50)
 165 
                    if (vzdalenost<55)
139 
                    {
 166 
                    {
140 
                        go(M1, 20);
 167 
                        go(M1, 30);
141 
                        go(M2, 40);
 168 
                        go(M2, 60);
142 
                    }
 169 
                    }
143 
                    else
 170 
                    else
144 
                    {
 171 
                    {
145 
                        go(M1, 40);
 172 
                        go(M1, 60);
146 
                        go(M2, 20);
 173 
                        go(M2, 30);
147 
                    }
 174 
                    }
148 
                }
 175 
                }
149 
                else
 176 
                else
150 
                {
 177 
                {
- 
 
 178 
                    filtr++;
- 
 
 179 
                    if (filtr>5)
- 
 
 180 
                    {
151 
                    go(M1, 0);  // zastav, neni videt doprovod
 181 
                        go(M1, 1);  // zastav, neni videt doprovod
152 
                    go(M2, 0);
 182 
                        go(M2, 1);
- 
 
 183 
                        filtr=6;
- 
 
 184 
                    }
153 
                };
 185 
                };
- 
 
 186 
                FindNearestCrossing();
154 
                break;
 187 
                break;
155 
        }
 188 
        }
156 
    };
 189 
    };
157 
 
 190 
 
158 
	close(file);
 191 
	close(file);
Line 191... Line 224...
191 
void *gps(void *unused)
 224 
void *gps(void *unused)
192 
{
 225 
{
193 
    FILE *pRS232;
 226 
    FILE *pRS232;
194 
    double N, E, pomN, pomE, nn, ee;
 227 
    double N, E, pomN, pomE, nn, ee;
195 
 
 228 
 
196 
    pRS232 = fopen("/dev/ttyS1","r");
 - 
 
197 
 
 - 
 
198 
    while(true)
 229 
    while(true)
199 
    {
 230 
    {
- 
 
 231 
        pRS232 = fopen("/dev/ttyS1","r");
200 
        fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
 232 
        fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
- 
 
 233 
        fclose(pRS232);
201 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
 234 
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
202 
        pomN=(N-nn*100)/60+nn;
 235 
        pomN=(N-nn*100)/60+nn;
203 
        ee=ldiv((long)E,100).quot;
 236 
        ee=ldiv((long)E,100).quot;
204 
        pomE=(E-ee*100)/60+ee;
 237 
        pomE=(E-ee*100)/60+ee;
205 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
 238 
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
206 
        n=pomN; e=pomE;
 239 
        n=pomN; e=pomE;
207 
        pthread_mutex_unlock(&mutex);
 240 
        pthread_mutex_unlock(&mutex);
208 
        usleep(500000);                     // NMEA nechodi castejc nez 1x za 1s
 241 
        usleep(800000);                     // NMEA nechodi castejc nez 1x za 1s
209 
    }
 242 
    }
210 
}
 243 
}
211 
 
 244 
 
212 
/*
 245 
/*
213 
void *sensors(void *unused)
 246 
void *sensors(void *unused)