| /roboti/Robotour/SW/vector/geocalc.cpp |
|---|
| 0,0 → 1,118 |
| /* Navigation code */ |
| #include "geocalc.h" |
| double GeoCalc::EllipsoidDistance(double lat1, double lon1, double lat2, double lon2) |
| { |
| double distance = 0.0; |
| double faz, baz; |
| double r = 1.0 - GEO::FLATTENING; |
| double tu1, tu2, cu1, su1, cu2, x, sx, cx, sy, cy, y, sa, c2a, cz, e, c, d; |
| double cosy1, cosy2; |
| distance = 0.0; |
| if((lon1 == lon2) && (lat1 == lat2)) return distance; |
| lon1 *= GEO::DE2RA; |
| lon2 *= GEO::DE2RA; |
| lat1 *= GEO::DE2RA; |
| lat2 *= GEO::DE2RA; |
| cosy1 = cos(lat1); |
| cosy2 = cos(lat2); |
| if(cosy1 == 0.0) cosy1 = 0.0000000001; |
| if(cosy2 == 0.0) cosy2 = 0.0000000001; |
| tu1 = r * sin(lat1) / cosy1; |
| tu2 = r * sin(lat2) / cosy2; |
| cu1 = 1.0 / sqrt(tu1 * tu1 + 1.0); |
| su1 = cu1 * tu1; |
| cu2 = 1.0 / sqrt(tu2 * tu2 + 1.0); |
| x = lon2 - lon1; |
| distance = cu1 * cu2; |
| baz = distance * tu2; |
| faz = baz * tu1; |
| do { |
| sx = sin(x); |
| cx = cos(x); |
| tu1 = cu2 * sx; |
| tu2 = baz - su1 * cu2 * cx; |
| sy = sqrt(tu1 * tu1 + tu2 * tu2); |
| cy = distance * cx + faz; |
| y = atan2(sy, cy); |
| sa = distance * sx / sy; |
| c2a = -sa * sa + 1.0; |
| cz = faz + faz; |
| if(c2a > 0.0) cz = -cz / c2a + cy; |
| e = cz * cz * 2. - 1.0; |
| c = ((-3.0 * c2a + 4.0) * GEO::FLATTENING + 4.0) * c2a * GEO::FLATTENING / 16.0; |
| d = x; |
| x = ((e * cy * c + cz) * sy * c + y) * sa; |
| x = (1.0 - c) * x * GEO::FLATTENING + lon2 - lon1; |
| } while(fabs(d - x) > GEO::EPS); |
| x = sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + 1.0; |
| x = (x - 2.0) / x; |
| c = 1.0 - x; |
| c = (x * x / 4.0 + 1.0) / c; |
| d = (0.375 * x * x - 1.0) * x; |
| x = e * cy; |
| distance = 1.0 - e - e; |
| distance = ((((sy * sy * 4.0 - 3.0) * |
| distance * cz * d / 6.0 - x) * d / 4.0 + cz) * sy * d + y) * c * GEO::ERAD * r; |
| return distance; |
| } |
| double GeoCalc::GCAzimuth(double lat1, double lon1, double lat2, double lon2) |
| { |
| double result = 0.0; |
| INT32 ilat1 = (INT32)(0.50 + lat1 * 360000.0); |
| INT32 ilat2 = (INT32)(0.50 + lat2 * 360000.0); |
| INT32 ilon1 = (INT32)(0.50 + lon1 * 360000.0); |
| INT32 ilon2 = (INT32)(0.50 + lon2 * 360000.0); |
| lat1 *= GEO::DE2RA; |
| lon1 *= GEO::DE2RA; |
| lat2 *= GEO::DE2RA; |
| lon2 *= GEO::DE2RA; |
| if ((ilat1 == ilat2) && (ilon1 == ilon2)) |
| { |
| return result; |
| } |
| else if (ilon1 == ilon2) |
| { |
| if (ilat1 > ilat2) |
| result = 180.0; |
| } |
| else |
| { |
| double c = acos(sin(lat2)*sin(lat1) + cos(lat2)*cos(lat1)*cos((lon2-lon1))); |
| double A = asin(cos(lat2)*sin((lon2-lon1))/sin(c)); |
| result = (A * GEO::RA2DE); |
| if ((ilat2 > ilat1) && (ilon2 > ilon1)) |
| { |
| } |
| else if ((ilat2 < ilat1) && (ilon2 < ilon1)) |
| { |
| result = 180.0 - result; |
| } |
| else if ((ilat2 < ilat1) && (ilon2 > ilon1)) |
| { |
| result = 180.0 - result; |
| } |
| else if ((ilat2 > ilat1) && (ilon2 < ilon1)) |
| { |
| result += 360.0; |
| } |
| } |
| return result; |
| } |
| /roboti/Robotour/SW/vector/geocalc.h |
|---|
| 0,0 → 1,27 |
| #include <iostream> |
| #include "math.h" |
| typedef signed int INT32; |
| namespace GEO { |
| const double PI = 3.14159265359; |
| const double PIOVER2 = PI/2.0; |
| const double TWOPI = 6.28318530718; |
| const double DE2RA = 0.01745329252; |
| const double RA2DE = 57.2957795129; |
| const double ERAD = 6378.137; |
| const double FLATTENING = 1.000000/298.257223563;// Earth flattening (WGS84) |
| const double EPS = 0.000000000005; |
| } |
| using namespace std; |
| class GeoCalc { |
| public: |
| // great circle method |
| static double GCAzimuth(double lat1, double lon1, double lat2, double lon2); |
| // ellipsoid methods |
| static double EllipsoidDistance(double lat1, double lon1, double lat2, double lon2); |
| }; |
| /roboti/Robotour/SW/vector/main.cpp |
|---|
| 0,0 → 1,9 |
| #include <iostream> |
| using namespace std; |
| int main() |
| { |
| cout << "Hello world!" << endl; |
| return 0; |
| } |
| /roboti/Robotour/SW/vector/vector.cbp |
|---|
| 0,0 → 1,42 |
| <?xml version="1.0" encoding="UTF-8" standalone="yes" ?> |
| <CodeBlocks_project_file> |
| <FileVersion major="1" minor="6" /> |
| <Project> |
| <Option title="vector" /> |
| <Option pch_mode="2" /> |
| <Option compiler="atmel_gnu_gcc_compiler" /> |
| <Build> |
| <Target title="Debug"> |
| <Option output="bin\Debug\vector" prefix_auto="1" extension_auto="1" /> |
| <Option object_output="obj\Debug\" /> |
| <Option type="1" /> |
| <Option compiler="atmel_gnu_gcc_compiler" /> |
| <Compiler> |
| <Add option="-g" /> |
| </Compiler> |
| </Target> |
| <Target title="Release"> |
| <Option output="bin\Release\vector" prefix_auto="1" extension_auto="1" /> |
| <Option object_output="obj\Release\" /> |
| <Option type="1" /> |
| <Option compiler="atmel_gnu_gcc_compiler" /> |
| <Compiler> |
| <Add option="-O2" /> |
| </Compiler> |
| <Linker> |
| <Add option="-s" /> |
| </Linker> |
| </Target> |
| </Build> |
| <Compiler> |
| <Add option="-Wall" /> |
| <Add option="-fexceptions" /> |
| </Compiler> |
| <Unit filename="main.cpp" /> |
| <Extensions> |
| <code_completion /> |
| <envvars /> |
| <debugger /> |
| </Extensions> |
| </Project> |
| </CodeBlocks_project_file> |
| /roboti/Robotour/SW/vector/vector.cpp |
|---|
| 0,0 → 1,159 |
| /*****************************************************************************/ |
| /* |
| * 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 PIC_Addr 0x50 // 0xA0 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); |
| while(true) |
| { |
| int n; |
| char Buf[64]; |
| for(n=-127; n<=127;n++) |
| { |
| 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]); |
| printf("+%X",Buf[2]); |
| usleep(300000); |
| /* |
| Buf[0]=(char)n; |
| I2C_addr(PIC_Addr); |
| Buf[0]=command; |
| write(file, Buf, 1); |
| read(file, Buf, 1); |
| printf("*%X",Buf[0]); |
| usleep(300000); |
| */ |
| }; |
| /* |
| for(n=127; n>=-127;n--) |
| { |
| Buf[0]=(char)n; |
| I2C_addr(PIC_Addr); |
| Buf[0]=command; |
| write(file, Buf, 1); |
| usleep(300000); |
| }; |
| */ |
| }; |
| close(file); |
| pthread_join(thread_1, NULL); |
| pthread_join(thread_2, NULL); |
| return 0; |
| } |
| void *print_tele(void *unused) |
| { |
| while(true) |
| { |
| pRouraI = fopen("/home/ble/pipe","r"); |
| command=fgetc(pRouraI); |
| fclose(pRouraI); |
| pRouraO = fopen("/home/ble/pipe","w"); |
| fprintf(pRouraO,"Vzdalenost: %u cm Command: %x\n",vzdalenost,ble); |
| printf("Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000); |
| printf("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]; |
| } |
| } |