Subversion Repositories svnkaklik

Rev

Rev 371 | Go to most recent revision | Show entire file | Ignore 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 
}