WebSVN – svnkaklik – Diff – /roboti/Robotour/SW/vector/vector.cpp


/*****************************************************************************/
/*
 *  vector.cpp - Control program for Vector robot
 *
 *      Copyright (C) 2007 KAKL
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 2 of the License, or
 *      (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 */
/*****************************************************************************/
 
#include <iostream>
#include <getopt.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "linux/i2c-dev.h"
#include "linux/i2c.h"
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include "geocalc.h"
#include "track.h"
 
using namespace std;
 
#define CMPS03_SOFTWARE_REVISION 0x0
#define SRF02_SOFTWARE_REVISION 0x0
 
#define BC_Addr	    0x0B
#define US_Addr	    0x70 // 0xE0 in fact
#define M1	0x50 // 0xA0 in fact
#define M2	0x51 // 0xA2 in fact
 
char vystup[50];
pthread_t thread_1, thread_2, thread_3;
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
char command,ble;
int file;
double n, e;
 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
void *print_tele(void *unused);
void *gps(void *unused);
//void *sensors(void *unused);
 
void I2C_addr (int Addr)
{
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
    {
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
        exit(-5);
    }
}
 
void go (int Addr, int speed)
{
    char Buf[1];
 
    I2C_addr (Addr);
    Buf[0]=speed;
    write(file, Buf, 1);
}
 
int main(int argc, char *argv[], char *envp[])
{
	fprintf(stdout, "**** Vector Control Programm ****\n");
 
	file = open("/dev/i2c-0", O_RDWR);
	if (file < 0)
	{
		cerr << "Could not open /dev/i2c-0." << endl;
		return -1;
	}
 
    pthread_create(&thread_1, NULL, print_tele, NULL);
    pthread_create(&thread_3, NULL, gps, NULL);
//    pthread_create(&thread_2, NULL, sensors, NULL);
 
    char Buf[64];
    command=0;
 
    while(true)
    {
        switch (command)
        {
            case 'f':   // forward
                go(M1, 70);
                go(M2, 70);
                command=0;
                break;
            case 'b':   // backward
                go(M1, -70);
                go(M2, -70);
                command=0;
                break;
            case 'v':   // stop
                go(M1, 0);
                go(M2, 0);
                command=0;
                break;
            case 's':   // stop
                go(M1, 1);
                go(M2, 1);
                command=0;
                break;
            case 'g':
usleep(180000);
                I2C_addr(US_Addr);
                Buf[0]=0x0;
                Buf[1]=0x51;
                write(file, Buf, 2);
                usleep(80000);
                read(file, Buf, 3);
                vzdalenost=(Buf[1]*256+Buf[2]);
                if ((vzdalenost>50)&&(vzdalenost<80))
                {
                    go(M1, 20);
                    go(M2, 20);
                    break;
                };
                if ((vzdalenost>30)&&(vzdalenost<120))
                {
                    if (vzdalenost<50)
                    {
                        go(M1, 20);
                        go(M2, 40);
                    }
                    else
                    {
                        go(M1, 40);
                        go(M2, 20);
                    }
                }
                else
                {
                    go(M1, 0);  // zastav, neni videt doprovod
                    go(M2, 0);
                };
                break;
        }
    };
 
	close(file);
    pthread_join(thread_1, NULL);
    pthread_join(thread_2, NULL);
    pthread_join(thread_3, NULL);
 
	return 0;
}
 
 
void *print_tele(void *unused)
{
    char string[2];
 
    while(true)
    {
        pRouraI = fopen("/home/ble/pipe","r");
        command=fgetc(pRouraI);
        string[0]=command;
        string[1]=0;
        fclose(pRouraI);
        pRouraO = fopen("/home/ble/pipe","w");
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
 
    pthread_mutex_lock(&mutex);
   	fprintf(pRouraO,"%f N %f E\n", n, e);
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
    pthread_mutex_unlock(&mutex);
 
        fclose(pRouraO);
    }
}
 
void *gps(void *unused)
{
    FILE *pRS232;
    double N, E, pomN, pomE, nn, ee;
 
    pRS232 = fopen("/dev/ttyS1","r");
 
    while(true)
    {
        fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
        nn=ldiv((long)N,100).quot;   // prepocet DDMM.MM na DD.DD
        pomN=(N-nn*100)/60+nn;
        ee=ldiv((long)E,100).quot;
        pomE=(E-ee*100)/60+ee;
        pthread_mutex_lock(&mutex);         // prepis souradnic do sdilenych promennych
        n=pomN; e=pomE;
        pthread_mutex_unlock(&mutex);
        usleep(500000);                     // NMEA nechodi castejc nez 1x za 1s
    }
}
 
/*
void *sensors(void *unused)
{
	char Buf[64];
 
    while(true)
    {
        I2C_addr(US_Addr);
        Buf[0]=0x0;
        Buf[1]=0x51;
        write(file, Buf, 2);
        usleep(80000);
        read(file, Buf, 3);
        vzdalenost=(Buf[1]*256+Buf[2]);
        usleep(300000);
 
        I2C_addr(PIC_Addr);
        Buf[0]=command;
        write(file, Buf, 1);
        read(file, Buf, 1);
        ble=Buf[0];
    }
}
*/

Rev 367	Rev 369
1	`/*****************************************************************************/`	1	`/*****************************************************************************/`
2	`/*`	2	`/*`
3	`* vector.cpp - Control program for Vector robot`	3	`* vector.cpp - Control program for Vector robot`
4	`*`	4	`*`
5	`* Copyright (C) 2007 KAKL`	5	`* Copyright (C) 2007 KAKL`
6	`*`	6	`*`
7	`* This program is free software; you can redistribute it and/or modify`	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`	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`	9	`* the Free Software Foundation; either version 2 of the License, or`
10	`* (at your option) any later version.`	10	`* (at your option) any later version.`
11	`*`	11	`*`
12	`* This program is distributed in the hope that it will be useful,`	12	`* This program is distributed in the hope that it will be useful,`
13	`* but WITHOUT ANY WARRANTY; without even the implied warranty of`	13	`* but WITHOUT ANY WARRANTY; without even the implied warranty of`
14	`* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the`	14	`* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the`
15	`* GNU General Public License for more details.`	15	`* GNU General Public License for more details.`
16	`*`	16	`*`
17	`*/`	17	`*/`
18	`/*****************************************************************************/`	18	`/*****************************************************************************/`
19		19
20	`#include <iostream>`	20	`#include <iostream>`
21	`#include <getopt.h>`	21	`#include <getopt.h>`
22	`#include <errno.h>`	22	`#include <errno.h>`
23	`#include <string.h>`	23	`#include <string.h>`
24	`#include <pthread.h>`	24	`#include <pthread.h>`
25	`#include <stdio.h>`	25	`#include <stdio.h>`
26	`#include <stdlib.h>`	26	`#include <stdlib.h>`
27	`#include <unistd.h>`	27	`#include <unistd.h>`
28	`#include "linux/i2c-dev.h"`	28	`#include "linux/i2c-dev.h"`
29	`#include "linux/i2c.h"`	29	`#include "linux/i2c.h"`
30	`#include <sys/ioctl.h>`	30	`#include <sys/ioctl.h>`
31	`#include <sys/types.h>`	31	`#include <sys/types.h>`
32	`#include <sys/stat.h>`	32	`#include <sys/stat.h>`
33	`#include <fcntl.h>`	33	`#include <fcntl.h>`
34	`#include "geocalc.h"`	34	`#include "geocalc.h"`
-		35	`#include "track.h"`
35		36
36	`using namespace std;`	37	`using namespace std;`
37		38
38	`#define CMPS03_SOFTWARE_REVISION 0x0`	39	`#define CMPS03_SOFTWARE_REVISION 0x0`
39	`#define SRF02_SOFTWARE_REVISION 0x0`	40	`#define SRF02_SOFTWARE_REVISION 0x0`
40		41
41	`#define BC_Addr 0x0B`	42	`#define BC_Addr 0x0B`
42	`#define US_Addr 0x70 // 0xE0 in fact`	43	`#define US_Addr 0x70 // 0xE0 in fact`
43	`#define M1 0x50 // 0xA0 in fact`	44	`#define M1 0x50 // 0xA0 in fact`
44	`#define M2 0x51 // 0xA2 in fact`	45	`#define M2 0x51 // 0xA2 in fact`
45		46
46	`char vystup[50];`	47	`char vystup[50];`
47	`pthread_t thread_1, thread_2, thread_3;`	48	`pthread_t thread_1, thread_2, thread_3;`
48	`FILE pRouraO,pRouraI;`	49	`FILE pRouraO,pRouraI;`
49	`unsigned int vzdalenost;`	50	`unsigned int vzdalenost;`
50	`char command,ble;`	51	`char command,ble;`
51	`int file;`	52	`int file;`
52	`double n, e;`	53	`double n, e;`
53		54
54	`pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;`	55	`pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;`
55		56
56	`void print_tele(void unused);`	57	`void print_tele(void unused);`
57	`void gps(void unused);`	58	`void gps(void unused);`
58	`//void sensors(void unused);`	59	`//void sensors(void unused);`
59		60
60	`void I2C_addr (int Addr)`	61	`void I2C_addr (int Addr)`
61	`{`	62	`{`
62	`if (ioctl(file, I2C_SLAVE, Addr) == -1)`	63	`if (ioctl(file, I2C_SLAVE, Addr) == -1)`
63	`{`	64	`{`
64	`fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);`	65	`fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);`
65	`exit(-5);`	66	`exit(-5);`
66	`}`	67	`}`
67	`}`	68	`}`
68		69
69	`void go (int Addr, int speed)`	70	`void go (int Addr, int speed)`
70	`{`	71	`{`
71	`char Buf[1];`	72	`char Buf[1];`
72		73
73	`I2C_addr (Addr);`	74	`I2C_addr (Addr);`
74	`Buf[0]=speed;`	75	`Buf[0]=speed;`
75	`write(file, Buf, 1);`	76	`write(file, Buf, 1);`
76	`}`	77	`}`
77		78
78	`int main(int argc, char argv[], char envp[])`	79	`int main(int argc, char argv[], char envp[])`
79	`{`	80	`{`
80	`fprintf(stdout, "** Vector Control Programm **\n");`	81	`fprintf(stdout, "** Vector Control Programm **\n");`
81		82
82	`file = open("/dev/i2c-0", O_RDWR);`	83	`file = open("/dev/i2c-0", O_RDWR);`
83	`if (file < 0)`	84	`if (file < 0)`
84	`{`	85	`{`
85	`cerr << "Could not open /dev/i2c-0." << endl;`	86	`cerr << "Could not open /dev/i2c-0." << endl;`
86	`return -1;`	87	`return -1;`
87	`}`	88	`}`
88		89
89	`pthread_create(&thread_1, NULL, print_tele, NULL);`	90	`pthread_create(&thread_1, NULL, print_tele, NULL);`
-		91	`pthread_create(&thread_3, NULL, gps, NULL);`
90	`// pthread_create(&thread_2, NULL, sensors, NULL);`	92	`// pthread_create(&thread_2, NULL, sensors, NULL);`
91		93
92	`char Buf[64];`	94	`char Buf[64];`
93	`command=0;`	95	`command=0;`
94		96
95	`while(true)`	97	`while(true)`
96	`{`	98	`{`
97	`switch (command)`	99	`switch (command)`
98	`{`	100	`{`
99	`case 'f': // forward`	101	`case 'f': // forward`
100	`go(M1, 20);`	102	`go(M1, 70);`
101	`go(M2, 20);`	103	`go(M2, 70);`
102	`command=0;`	104	`command=0;`
103	`break;`	105	`break;`
104	`case 'b': // backward`	106	`case 'b': // backward`
105	`go(M1, -20);`	107	`go(M1, -70);`
106	`go(M2, -20);`	108	`go(M2, -70);`
107	`command=0;`	109	`command=0;`
108	`break;`	110	`break;`
109	`case 's': // stop`	111	`case 'v': // stop`
110	`go(M1, 0);`	112	`go(M1, 0);`
111	`go(M2, 0);`	113	`go(M2, 0);`
112	`command=0;`	114	`command=0;`
113	`break;`	115	`break;`
-		116	`case 's': // stop`
-		117	`go(M1, 1);`
-		118	`go(M2, 1);`
-		119	`command=0;`
-		120	`break;`
114	`case 'g':`	121	`case 'g':`
115	`usleep(180000);`	122	`usleep(180000);`
116	`I2C_addr(US_Addr);`	123	`I2C_addr(US_Addr);`
117	`Buf[0]=0x0;`	124	`Buf[0]=0x0;`
118	`Buf[1]=0x51;`	125	`Buf[1]=0x51;`
119	`write(file, Buf, 2);`	126	`write(file, Buf, 2);`
120	`usleep(80000);`	127	`usleep(80000);`
121	`read(file, Buf, 3);`	128	`read(file, Buf, 3);`
122	`vzdalenost=(Buf[1]*256+Buf[2]);`	129	`vzdalenost=(Buf[1]*256+Buf[2]);`
123	`if ((vzdalenost>50)&&(vzdalenost<80))`	130	`if ((vzdalenost>50)&&(vzdalenost<80))`
124	`{`	131	`{`
125	`go(M1, 20);`	132	`go(M1, 20);`
126	`go(M2, 20);`	133	`go(M2, 20);`
127	`break;`	134	`break;`
128	`};`	135	`};`
129	`if ((vzdalenost>30)&&(vzdalenost<120))`	136	`if ((vzdalenost>30)&&(vzdalenost<120))`
130	`{`	137	`{`
131	`if (vzdalenost<50)`	138	`if (vzdalenost<50)`
132	`{`	139	`{`
133	`go(M1, 20);`	140	`go(M1, 20);`
134	`go(M2, 40);`	141	`go(M2, 40);`
135	`}`	142	`}`
136	`else`	143	`else`
137	`{`	144	`{`
138	`go(M1, 40);`	145	`go(M1, 40);`
139	`go(M2, 20);`	146	`go(M2, 20);`
140	`}`	147	`}`
141	`}`	148	`}`
142	`else`	149	`else`
143	`{`	150	`{`
144	`go(M1, 0); // zastav, neni videt doprovod`	151	`go(M1, 0); // zastav, neni videt doprovod`
145	`go(M2, 0);`	152	`go(M2, 0);`
146	`};`	153	`};`
147	`break;`	154	`break;`
148	`}`	155	`}`
149	`};`	156	`};`
150		157
151	`close(file);`	158	`close(file);`
152	`pthread_join(thread_1, NULL);`	159	`pthread_join(thread_1, NULL);`
153	`pthread_join(thread_2, NULL);`	160	`pthread_join(thread_2, NULL);`
-		161	`pthread_join(thread_3, NULL);`
154		162
155	`return 0;`	163	`return 0;`
156	`}`	164	`}`
157		165
158		166
159	`void print_tele(void unused)`	167	`void print_tele(void unused)`
160	`{`	168	`{`
161	`char string[2];`	169	`char string[2];`
162		170
163	`while(true)`	171	`while(true)`
164	`{`	172	`{`
165	`pRouraI = fopen("/home/ble/pipe","r");`	173	`pRouraI = fopen("/home/ble/pipe","r");`
166	`command=fgetc(pRouraI);`	174	`command=fgetc(pRouraI);`
167	`string[0]=command;`	175	`string[0]=command;`
168	`string[1]=0;`	176	`string[1]=0;`
169	`fclose(pRouraI);`	177	`fclose(pRouraI);`
170	`pRouraO = fopen("/home/ble/pipe","w");`	178	`pRouraO = fopen("/home/ble/pipe","w");`
171	`fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);`	179	`fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);`
172		180
173	`pthread_mutex_lock(&mutex);`	181	`pthread_mutex_lock(&mutex);`
174	`fprintf(pRouraO,"%f N %f E\n", n, e);`	182	`fprintf(pRouraO,"%f N %f E\n", n, e);`
175	`fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, 49.266667, 14.716667));`	183	`fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));`
176	`fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667));`	184	`fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));`
177	`pthread_mutex_unlock(&mutex);`	185	`pthread_mutex_unlock(&mutex);`
178		186
179	`fclose(pRouraO);`	187	`fclose(pRouraO);`
180	`}`	188	`}`
181	`}`	189	`}`
182		190
183	`void gps(void unused)`	191	`void gps(void unused)`
184	`{`	192	`{`
185	`FILE *pRS232;`	193	`FILE *pRS232;`
186	`double pomN, pomE;`	194	`double N, E, pomN, pomE, nn, ee;`
187		195
188	`pRS232 = fopen("/dev/ttyS1","r");`	196	`pRS232 = fopen("/dev/ttyS1","r");`
189		197
190	`while(true)`	198	`while(true)`
191	`{`	199	`{`
192	`fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE);`	200	`fscanf(pRS232,"$GPGGA,%f,%lf,N,%lf,E,", &N, &E); // parser NMEA`
-		201	`nn=ldiv((long)N,100).quot; // prepocet DDMM.MM na DD.DD`
-		202	`pomN=(N-nn*100)/60+nn;`
-		203	`ee=ldiv((long)E,100).quot;`
193	`pthread_mutex_lock(&mutex);`	204	`pomE=(E-ee*100)/60+ee;`
-		205	`pthread_mutex_lock(&mutex); // prepis souradnic do sdilenych promennych`
194	`n=pomN; e=pomE;`	206	`n=pomN; e=pomE;`
195	`pthread_mutex_unlock(&mutex);`	207	`pthread_mutex_unlock(&mutex);`
196	`usleep(500000);`	208	`usleep(500000); // NMEA nechodi castejc nez 1x za 1s`
197	`}`	209	`}`
198	`}`	210	`}`
199		211
200	`/*`	212	`/*`
201	`void sensors(void unused)`	213	`void sensors(void unused)`
202	`{`	214	`{`
203	`char Buf[64];`	215	`char Buf[64];`
204		216
205	`while(true)`	217	`while(true)`
206	`{`	218	`{`
207	`I2C_addr(US_Addr);`	219	`I2C_addr(US_Addr);`
208	`Buf[0]=0x0;`	220	`Buf[0]=0x0;`
209	`Buf[1]=0x51;`	221	`Buf[1]=0x51;`
210	`write(file, Buf, 2);`	222	`write(file, Buf, 2);`
211	`usleep(80000);`	223	`usleep(80000);`
212	`read(file, Buf, 3);`	224	`read(file, Buf, 3);`
213	`vzdalenost=(Buf[1]*256+Buf[2]);`	225	`vzdalenost=(Buf[1]*256+Buf[2]);`
214	`usleep(300000);`	226	`usleep(300000);`
215		227
216	`I2C_addr(PIC_Addr);`	228	`I2C_addr(PIC_Addr);`
217	`Buf[0]=command;`	229	`Buf[0]=command;`
218	`write(file, Buf, 1);`	230	`write(file, Buf, 1);`
219	`read(file, Buf, 1);`	231	`read(file, Buf, 1);`
220	`ble=Buf[0];`	232	`ble=Buf[0];`
221	`}`	233	`}`
222	`}`	234	`}`
223	`*/`	235	`*/`

Subversion Repositories svnkaklik

(root)/roboti/Robotour/SW/vector/vector.cpp – Rev 367 → 369