Subversion Repositories svnkaklik

Rev

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

Rev 371 Rev 372
Line 34... Line 34...
34
#include "geocalc.h"
34
#include "geocalc.h"
35
#include "track.h"
35
#include "track.h"
36
 
36
 
37
using namespace std;
37
using namespace std;
38
 
38
 
39
#define CMPS03_SOFTWARE_REVISION 0x0
-
 
40
#define SRF02_SOFTWARE_REVISION 0x0
-
 
41
 
39
 
42
#define BC_Addr	    0x0B
40
#define BC_Addr	    0x0B
43
#define US3_Addr	    0x70 // 0xE0 in fact; Sonar na doprovod
41
#define US3_Addr	0x70    // 0xE0 in fact; Sonar na doprovod
-
 
42
#define CMPS_Addr   0x60    // 0xC0
44
#define M1	0x50 // 0xA0 in fact
43
#define M1	0x50 // 0xA0 in fact
45
#define M2	0x51 // 0xA2 in fact
44
#define M2	0x51 // 0xA2 in fact
46
 
45
 
-
 
46
#define SEVER 0
-
 
47
 
47
char vystup[50];
48
char vystup[50];
48
pthread_t thread_1, thread_2, thread_3;
49
pthread_t thread_1, thread_2, thread_3;
49
FILE *pRouraO,*pRouraI;
50
FILE *pRouraO,*pRouraI;
50
unsigned int vzdalenost;
51
unsigned int vzdalenost;
51
char command,ble;
52
char command,ble;
52
int file;
53
int file;
53
double n, e;
54
double n, e;
-
 
55
unsigned char azimut_mag;
-
 
56
 
54
 
57
 
55
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
58
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
56
 
59
 
57
void *print_tele(void *unused);
60
void *print_tele(void *unused);
58
void *gps(void *unused);
61
void *gps(void *unused);
Line 65... Line 68...
65
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
68
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
66
        exit(-5);
69
        exit(-5);
67
    }
70
    }
68
}
71
}
69
 
72
 
70
void go (int Addr, int speed)
73
void go (int Addr, int speed) // nastavi rychlost motoru
71
{
74
{
72
    char Buf[1];
75
    char Buf[1];
73
 
76
 
74
    I2C_addr (Addr);
77
    I2C_addr (Addr);
75
    Buf[0]=speed;
78
    Buf[0]=speed;
76
    write(file, Buf, 1);
79
    write(file, Buf, 1);
77
}
80
}
78
 
81
 
79
unsigned int echo(int Addr)
82
unsigned int echo(int Addr)  // precte vzdalenost z US cidla
80
{
83
{
81
    char Buf[3];
84
    char Buf[3];
82
 
85
 
83
    I2C_addr(Addr);
86
    I2C_addr(Addr);
84
    Buf[0]=0x0;
87
    Buf[0]=0x0;
Line 103... Line 106...
103
            break;
106
            break;
104
        }
107
        }
105
    } while(++n<POINTS);
108
    } while(++n<POINTS);
106
}
109
}
107
 
110
 
108
int main(int argc, char *argv[], char *envp[])
111
int i2c_init()   // zinicializuje i2c
109
{
112
{
110
    int filtr;
-
 
111
 
-
 
112
	fprintf(stdout, "**** Vector Control Programm ****\n");
-
 
113
 
-
 
114
	file = open("/dev/i2c-0", O_RDWR);
113
	file = open("/dev/i2c-0", O_RDWR);
115
	if (file < 0)
114
	if (file < 0)
116
	{
115
	{
117
		cerr << "Could not open /dev/i2c-0." << endl;
116
		cerr << "Could not open /dev/i2c-0." << endl;
118
		return -1;
117
		return -1;
119
	}
118
	}
-
 
119
	return 0;
-
 
120
}
-
 
121
 
-
 
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();
120
 
157
 
121
    pthread_create(&thread_1, NULL, print_tele, NULL);
158
    pthread_create(&thread_1, NULL, print_tele, NULL);
122
    pthread_create(&thread_3, NULL, gps, NULL);
159
    pthread_create(&thread_3, NULL, gps, NULL);
123
//    pthread_create(&thread_2, NULL, sensors, NULL);
160
//    pthread_create(&thread_2, NULL, sensors, NULL);
124
 
161
 
Line 137... Line 174...
137
            case 'b':   // backward
174
            case 'b':   // backward
138
                go(M1, -70);
175
                go(M1, -70);
139
                go(M2, -70);
176
                go(M2, -70);
140
                command=0;
177
                command=0;
141
                break;
178
                break;
-
 
179
            case 'v':   // volnobeh
-
 
180
                go(M1, -128);
-
 
181
                go(M2, -128);
-
 
182
                command=0;
-
 
183
                break;
142
            case 'v':   // stop
184
            case 's':   // stop
143
                go(M1, 0);
185
                go(M1, 0);
144
                go(M2, 0);
186
                go(M2, 0);
145
                command=0;
187
                command=0;
146
                break;
188
                break;
147
            case 's':   // stop
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);
148
                go(M1, 1);
194
                go(M1, 0);
149
                go(M2, 1);
195
                go(M2, 0);
150
                command=0;
196
                command=0;
151
                break;
197
                break;
-
 
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
 
152
            case 'g':
219
            case 'g':
153
usleep(180000);
220
    usleep(180000);  // simulace ostatnich  cidel (zdrzeni)
-
 
221
                azimut_mag=read_azimut_mag();
-
 
222
 
-
 
223
                FindNearestCrossing();
-
 
224
 
154
                vzdalenost=echo(US3_Addr);
225
                vzdalenost=echo(US3_Addr);
155
                if ((vzdalenost>60)&&(vzdalenost<80))
226
                if ((vzdalenost>60)&&(vzdalenost<80))
156
                {
227
                {
157
                    go(M1, 60);
228
                    go(M1, 60);
158
                    go(M2, 60);
229
                    go(M2, 60);
Line 181... Line 252...
181
                        go(M1, 1);  // zastav, neni videt doprovod
252
                        go(M1, 1);  // zastav, neni videt doprovod
182
                        go(M2, 1);
253
                        go(M2, 1);
183
                        filtr=6;
254
                        filtr=6;
184
                    }
255
                    }
185
                };
256
                };
186
                FindNearestCrossing();
-
 
187
                break;
257
                break;
188
        }
258
        }
189
    };
259
    };
190
 
260
 
191
	close(file);
261
	close(file);
Line 213... Line 283...
213
 
283
 
214
    pthread_mutex_lock(&mutex);
284
    pthread_mutex_lock(&mutex);
215
   	fprintf(pRouraO,"%f N %f E\n", n, e);
285
   	fprintf(pRouraO,"%f N %f E\n", n, e);
216
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
286
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
217
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(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));
-
 
288
	fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag);
218
    pthread_mutex_unlock(&mutex);
289
    pthread_mutex_unlock(&mutex);
219
 
290
 
220
        fclose(pRouraO);
291
        fclose(pRouraO);
221
    }
292
    }
222
}
293
}