| 36,15 → 36,14 |
|---|
| |
| using namespace std; |
| |
| #define CMPS03_SOFTWARE_REVISION 0x0 |
| #define SRF02_SOFTWARE_REVISION 0x0 |
| |
| #define BC_Addr 0x0B |
| #define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod |
| #define CMPS_Addr 0x60 // 0xC0 |
| #define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod |
| #define M1 0x50 // 0xA0 in fact |
| #define M2 0x51 // 0xA2 in fact |
| |
| #define SEVER 0 |
| |
| char vystup[50]; |
| pthread_t thread_1, thread_2, thread_3; |
| FILE *pRouraO,*pRouraI; |
| 52,9 → 51,7 |
|---|
| char command,ble; |
| int file; |
| double n, e; |
| unsigned char azimut_mag; |
| |
| |
| pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; |
| |
| void *print_tele(void *unused); |
| 70,7 → 67,7 |
|---|
| } |
| } |
| |
| void go (int Addr, int speed) // nastavi rychlost motoru |
| void go (int Addr, int speed) |
| { |
| char Buf[1]; |
| |
| 79,7 → 76,7 |
|---|
| write(file, Buf, 1); |
| } |
| |
| unsigned int echo(int Addr) // precte vzdalenost z US cidla |
| unsigned int echo(int Addr) |
| { |
| char Buf[3]; |
| |
| 108,8 → 105,12 |
|---|
| } while(++n<POINTS); |
| } |
| |
| int i2c_init() // zinicializuje i2c |
| int main(int argc, char *argv[], char *envp[]) |
| { |
| int filtr; |
| |
| fprintf(stdout, "**** Vector Control Programm ****\n"); |
| |
| file = open("/dev/i2c-0", O_RDWR); |
| if (file < 0) |
| { |
| 116,45 → 117,7 |
|---|
| cerr << "Could not open /dev/i2c-0." << endl; |
| return -1; |
| } |
| return 0; |
| } |
| |
| int read_azimut_mag() // precte azimut z kompasu |
| { |
| char Buf[3]; // promena pro manipulaci s i2c |
| I2C_addr(CMPS_Addr); |
| Buf[0]=1; |
| write(file,Buf,1); |
| read(file, Buf,1); |
| return Buf[0]; |
| } |
| |
| void turnL(unsigned char angle, signed char speed) // otoci robota o zadany uhel |
| { |
| int azimut; |
| |
| go(M1, speed); |
| go(M2, -speed); |
| azimut=read_azimut_mag(); |
| while(read_azimut_mag() >= (azimut + angle)) usleep(10000); |
| go(M1, 0); |
| go(M2, 0); |
| command=0; |
| } |
| |
| void turnR() // otoci robota o zadany uhel |
| { |
| } |
| |
| int main(int argc, char *argv[], char *envp[]) |
| { |
| int filtr; |
| signed char test; |
| |
| fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r"); |
| |
| i2c_init(); |
| |
| pthread_create(&thread_1, NULL, print_tele, NULL); |
| pthread_create(&thread_3, NULL, gps, NULL); |
| // pthread_create(&thread_2, NULL, sensors, NULL); |
| 176,52 → 139,18 |
|---|
| go(M2, -70); |
| command=0; |
| break; |
| case 'v': // volnobeh |
| go(M1, -128); |
| go(M2, -128); |
| command=0; |
| break; |
| case 's': // stop |
| case 'v': // stop |
| go(M1, 0); |
| go(M2, 0); |
| command=0; |
| break; |
| case 'a': // test otaceni |
| go(M1, 100); |
| go(M2, -100); |
| azimut_mag=read_azimut_mag(); |
| while(read_azimut_mag() >= (azimut_mag + 45)) usleep(10000); |
| go(M1, 0); |
| go(M2, 0); |
| case 's': // stop |
| go(M1, 1); |
| go(M2, 1); |
| command=0; |
| break; |
| |
| case 't': // test |
| for(test=0;test<127;test++) |
| { |
| go(M1, test); |
| go(M2, test); |
| usleep(10000); |
| }; |
| go(M1, 127); |
| go(M2, 127); |
| for(test=127;test>-128;test--) |
| { |
| go(M1, test); |
| go(M2, test); |
| usleep(10000); |
| }; |
| go(M1, -128); |
| go(M2, -128); |
| command=0; |
| break; |
| |
| case 'g': |
| usleep(180000); // simulace ostatnich cidel (zdrzeni) |
| azimut_mag=read_azimut_mag(); |
| |
| FindNearestCrossing(); |
| |
| usleep(180000); |
| vzdalenost=echo(US3_Addr); |
| if ((vzdalenost>60)&&(vzdalenost<80)) |
| { |
| 254,6 → 183,7 |
|---|
| filtr=6; |
| } |
| }; |
| FindNearestCrossing(); |
| break; |
| } |
| }; |
| 285,7 → 215,6 |
|---|
| 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)); |
| fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag); |
| pthread_mutex_unlock(&mutex); |
| |
| fclose(pRouraO); |