| 0,0 → 1,167 |
|---|
| /*****************************************************************************/ |
| /* |
| * 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_Addr 0x50 // 0xA0 in fact |
| #define M2_Addr 0x51 // 0xA2 in fact |
| |
| char vystup[30]; |
| pthread_t thread_1, thread_2; |
| FILE *pRouraO,*pRouraI; |
| unsigned int vzdalenost; |
| char command,ble; |
| int file; |
| |
| void *print_tele(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); |
| } |
| } |
| |
| 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=127; |
| |
| 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) |
| { |
| I2C_addr(M1_Addr); |
| Buf[0]=command; |
| write(file, Buf, 1); |
| I2C_addr(M2_Addr); |
| Buf[0]=command; |
| write(file, Buf, 1); |
| |
| command=127; |
| } |
| }; |
| |
| close(file); |
| pthread_join(thread_1, NULL); |
| pthread_join(thread_2, NULL); |
| |
| return 0; |
| } |
| |
| |
| void *print_tele(void *unused) |
| { |
| char i; |
| |
| 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; |
| } |
| |
| 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)); |
| fclose(pRouraO); |
| } |
| } |
| |
| /* |
| 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]; |
| } |
| } |
| */ |