| 40,17 → 40,21 |
|---|
| |
| #define BC_Addr 0x0B |
| #define US_Addr 0x70 // 0xE0 in fact |
| #define M1_Addr 0x50 // 0xA0 in fact |
| #define M2_Addr 0x51 // 0xA2 in fact |
| #define M1 0x50 // 0xA0 in fact |
| #define M2 0x51 // 0xA2 in fact |
| |
| char vystup[30]; |
| pthread_t thread_1, thread_2; |
| 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) |
| 62,6 → 66,15 |
|---|
| } |
| } |
| |
| 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"); |
| 77,29 → 90,61 |
|---|
| // pthread_create(&thread_2, NULL, sensors, NULL); |
| |
| char Buf[64]; |
| command=127; |
| command=0; |
| |
| 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); |
| |
| if (command!=127) |
| switch (command) |
| { |
| I2C_addr(M1_Addr); |
| Buf[0]=command; |
| write(file, Buf, 1); |
| I2C_addr(M2_Addr); |
| Buf[0]=command; |
| write(file, Buf, 1); |
| |
| command=127; |
| case 'f': // forward |
| go(M1, 20); |
| go(M2, 20); |
| command=0; |
| break; |
| case 'b': // backward |
| go(M1, -20); |
| go(M2, -20); |
| command=0; |
| break; |
| case 's': // stop |
| go(M1, 0); |
| go(M2, 0); |
| 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; |
| } |
| }; |
| |
| 113,34 → 158,45 |
|---|
| |
| void *print_tele(void *unused) |
| { |
| char i; |
| char string[2]; |
| |
| while(true) |
| { |
| pRouraI = fopen("/home/ble/pipe","r"); |
| i=fgetc(pRouraI); |
| switch (i) |
| { |
| case 'a': |
| command=0; |
| break; |
| case 'b': |
| command=20; |
| break; |
| case 'c': |
| command=-20; |
| break; |
| } |
| |
| command=fgetc(pRouraI); |
| string[0]=command; |
| string[1]=0; |
| fclose(pRouraI); |
| pRouraO = fopen("/home/ble/pipe","w"); |
| fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command); |
| fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000); |
| fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667)); |
| 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, 49.266667, 14.716667)); |
| fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667)); |
| pthread_mutex_unlock(&mutex); |
| |
| fclose(pRouraO); |
| } |
| } |
| |
| void *gps(void *unused) |
| { |
| FILE *pRS232; |
| double pomN, pomE; |
| |
| pRS232 = fopen("/dev/ttyS1","r"); |
| |
| while(true) |
| { |
| fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE); |
| pthread_mutex_lock(&mutex); |
| n=pomN; e=pomE; |
| pthread_mutex_unlock(&mutex); |
| usleep(500000); |
| } |
| } |
| |
| /* |
| void *sensors(void *unused) |
| { |