| 0,0 → 1,408 |
|---|
| /*****************************************************************************/ |
| /* |
| * 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" |
| #include "track.h" |
| |
| using namespace std; |
| |
| |
| #define BC_Addr 0x0B |
| #define US1_Addr (0xE2>>1) |
| #define US2_Addr (0xE4>>1) |
| #define US3_Addr (0xE6>>1) |
| #define CMPS_Addr (0xC0>>1) |
| #define M1 (0xA0>>1) |
| #define M2 (0xA2>>1) |
| |
| #define SEVER 122 |
| |
| char vystup[50]; |
| pthread_t thread_1, thread_2, thread_3; |
| FILE *pRouraO,*pRouraI; |
| unsigned int vzdalenost; |
| char command,ble; |
| int param; |
| int file; |
| double nord, east; |
| int done; // vlajka, ze se neco udelalo |
| int last_cross; // posledni krizovatka |
| |
| 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) // nastavi rychlost motoru |
| { |
| char Buf[1]; |
| |
| I2C_addr (Addr); |
| Buf[0]=speed; |
| write(file, Buf, 1); |
| } |
| |
| unsigned int echo(int Addr) // precte vzdalenost z US cidla |
| { |
| char Buf[3]; |
| |
| I2C_addr(Addr); |
| Buf[0]=0x0; |
| Buf[1]=0x51; |
| write(file, Buf, 2); |
| usleep(80000); |
| read(file, Buf, 3); |
| return (Buf[1]*256+Buf[2]); |
| } |
| |
| unsigned char 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]-SEVER); |
| } |
| |
| void calib() // kalibrace kompasu |
| { |
| char Buf[3]; // promena pro manipulaci s i2c |
| |
| I2C_addr(CMPS_Addr); |
| Buf[0]=15; |
| Buf[1]=0xFF; |
| write(file,Buf,2); |
| } |
| |
| void TL (unsigned char azimut2) |
| { |
| unsigned char azimut1; |
| |
| go(M1, 0); |
| go(M2, 120); |
| do |
| { |
| azimut1=read_azimut_mag(); |
| //printf("az1: %d - az2: %d\n", azimut1, azimut2); |
| usleep(10000); |
| } while( (azimut1!=azimut2)&&((azimut1+1)!=azimut2)&&((azimut1+2)!=azimut2)&&((azimut1+3)!=azimut2) ); |
| go(M1, 0); |
| go(M2, 0); |
| } |
| |
| void TR (unsigned char azimut2) |
| { |
| unsigned char azimut1; |
| |
| go(M1, 120); |
| go(M2, 0); |
| do |
| { |
| azimut1=read_azimut_mag(); |
| usleep(10000); |
| } while( (azimut1!=azimut2)&&((azimut1-1)!=azimut2)&&((azimut1-2)!=azimut2)&&((azimut1-3)!=azimut2) ); |
| go(M1, 0); |
| go(M2, 0); |
| } |
| |
| void FindNearestCrossing(void) |
| { |
| int n; |
| double dist, pomN, pomE; |
| |
| pthread_mutex_lock(&mutex); // prepis souradnic do pracovnich promennych |
| pomN=nord; pomE=east; |
| pthread_mutex_unlock(&mutex); |
| if ( GeoCalc::EllipsoidDistance(pomN, pomE, cros[last_cross].n, cros[last_cross].e)>((double)cros[n].dia+5) ) done=0; // znovu naviguj, pokud si dal nez... od krizovatky |
| n=0; |
| do |
| { |
| if (0==cros[n].id) break; |
| dist=GeoCalc::EllipsoidDistance(pomN, pomE, cros[n].n, cros[n].e); |
| if (dist <= (double)cros[n].dia) |
| { |
| //printf("Point ID: %d - Distance: %f\n",cros[n].id, dist); |
| if (done==0) |
| { |
| last_cross=n; |
| #include "nav.h" |
| done=1; |
| } |
| break; |
| } |
| } while(++n<POINTS); |
| } |
| |
| int i2c_init() // zinicializuje i2c |
| { |
| file = open("/dev/i2c-0", O_RDWR); |
| if (file < 0) |
| { |
| cerr << "Could not open /dev/i2c-0." << endl; |
| return -1; |
| } |
| return 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); |
| } |
| |
| 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(); |
| |
| last_cross=0; |
| command=0; |
| filtr=0; |
| done=0; |
| |
| pthread_create(&thread_1, NULL, print_tele, NULL); |
| pthread_create(&thread_3, NULL, gps, NULL); |
| // pthread_create(&thread_2, NULL, sensors, NULL); |
| |
| while(true) |
| { |
| switch (command) |
| { |
| case 'f': // forward |
| go(M1, param); |
| go(M2, param); |
| sleep(1); |
| command=0; |
| break; |
| |
| case 'b': // backward |
| go(M1, -param); |
| go(M2, -param); |
| sleep(1); |
| command=0; |
| break; |
| |
| case 'v': // volnobeh |
| go(M1, -128); |
| go(M2, -128); |
| command=0; |
| break; |
| |
| case 's': // stop |
| go(M1, 0); |
| go(M2, 0); |
| command=0; |
| break; |
| |
| case 'l': // left |
| TL(param); |
| command=0; |
| break; |
| |
| case 'r': // right |
| TR(param); |
| command=0; |
| break; |
| |
| case 'c': // kalibrace kompasu |
| calib(); |
| 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) |
| |
| //!!!KAKL FindNearestCrossing(); |
| |
| vzdalenost=echo(US3_Addr); |
| if ((vzdalenost>60)&&(vzdalenost<80)) |
| { |
| go(M1, 60); |
| go(M2, 60); |
| filtr=0; |
| break; |
| }; |
| if ((vzdalenost>30)&&(vzdalenost<130)) |
| { |
| filtr=0; |
| if (vzdalenost<55) |
| { |
| go(M1, 30); |
| go(M2, 60); |
| } |
| else |
| { |
| go(M1, 60); |
| go(M2, 30); |
| } |
| } |
| else |
| { |
| filtr++; |
| if (filtr>5) |
| { |
| go(M1, 0); // zastav, neni videt doprovod |
| go(M2, 0); |
| filtr=6; |
| } |
| }; |
| break; |
| } |
| }; |
| |
| close(file); |
| pthread_join(thread_1, NULL); |
| pthread_join(thread_2, NULL); |
| pthread_join(thread_3, NULL); |
| |
| return 0; |
| } |
| |
| |
| void *print_tele(void *unused) |
| { |
| char pom; |
| |
| while(true) |
| { |
| pRouraI = fopen("/home/ble/pipe","r"); |
| fscanf(pRouraI,"%1s%d", &pom, ¶m); |
| fclose(pRouraI); |
| if (pom!='i') command=pom; |
| |
| pRouraO = fopen("/home/ble/pipe","w"); |
| fprintf(pRouraO,"US: %u cm - ",vzdalenost); |
| |
| pthread_mutex_lock(&mutex); |
| fprintf(pRouraO,"%fN %fE - ", nord, east); |
| fprintf(pRouraO,"Bod:%d - ", last_cross); |
| fprintf(pRouraO,"Len:%.1f m /", GeoCalc::EllipsoidDistance(nord, east, cros[last_cross].n, cros[last_cross].e)); |
| fprintf(pRouraO,"Az:%.2f Deg - ", GeoCalc::GCAzimuth(nord, east, cros[last_cross].n, cros[last_cross].e)); |
| fprintf(pRouraO,"AzMag: %d (0-255)\n", read_azimut_mag()); |
| pthread_mutex_unlock(&mutex); |
| |
| fclose(pRouraO); |
| } |
| } |
| |
| void *gps(void *unused) |
| { |
| FILE *pRS232; |
| double N, E, pomN, pomE, nn, ee; |
| |
| while(true) |
| { |
| pRS232 = fopen("/dev/ttyS1","r"); |
| fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA |
| fclose(pRS232); |
| nn=ldiv((long)N,100).quot; // prepocet DDMM.MM na DD.DD |
| pomN=(N-nn*100)/60+nn; |
| ee=ldiv((long)E,100).quot; |
| pomE=(E-ee*100)/60+ee; |
| pthread_mutex_lock(&mutex); // prepis souradnic do sdilenych promennych |
| nord=pomN; east=pomE; |
| pthread_mutex_unlock(&mutex); |
| usleep(800000); // NMEA nechodi castejc nez 1x za 1s |
| } |
| } |
| |
| /* |
| 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]; |
| } |
| } |
| */ |