| 0,0 → 1,223 |
|---|
| /*****************************************************************************/ |
| /* |
| * 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" |
| |
| 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_2, NULL, sensors, NULL); |
| |
| char Buf[64]; |
| command=0; |
| |
| while(true) |
| { |
| switch (command) |
| { |
| 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; |
| } |
| }; |
| |
| close(file); |
| pthread_join(thread_1, NULL); |
| pthread_join(thread_2, 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, 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) |
| { |
| 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]; |
| } |
| } |
| */ |