Rev 379 | Blame | Last modification | View Log | Download
/*****************************************************************************/
/*
* 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, lus, rus;
char command,ble;
int param;
int file;
double nord, east;
int done; // vlajka, ze se neco udelalo
int last_cross; // posledni krizovatka
int korekce; // Byla korekce podle kompasu nebo zdi
int bod;
int start;
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) // vybere adresu cidla se kterym se bude komunikovat
{
if (ioctl(file, I2C_SLAVE, Addr) == -1)
{
fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
exit(-5);
}
}
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 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) // otoci robota na zadany azimut rotaci vlevo
{
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)&&((azimut1+4)!=azimut2) );
go(M1, 0);
go(M2, 0);
}
void TR (unsigned char azimut2) // otoci robota na zadany azimut rotaci v pravo
{
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)&&((azimut1-4)!=azimut2) );
go(M1, 0);
go(M2, 0);
}
void SlowLeft()
{
go(M1, 40);
go(M2, 60);
}
void SlowRight()
{
go(M1, 60);
go(M2, 40);
}
void FindNearestCrossing(void) // vykonej nejakou akci (zatoceni) pokud se robot nachazi pobliz nejakeho waypointu
{
int n;
double dist, pomN, pomE;
unsigned char azimut;
pthread_mutex_lock(&mutex); // prepis souradnic do pracovnich promennych
pomN=nord; pomE=east;
pthread_mutex_unlock(&mutex);
korekce=0;
// if (last_cross!=-1) 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);
korekce=1;
if (last_cross!=cros[n].id)
{
bod=cros[n].id;
#include "nav.h"
// done=1;
}
break;
}
} while(++n<POINTS);
}
/*
void FindWay(void) // koriguj smer podle kompasu
{
int n;
double dist, pomN, pomE;
pthread_mutex_lock(&mutex); // prepis souradnic do pracovnich promennych
pomN=nord; pomE=east;
pthread_mutex_unlock(&mutex);
// if (last_cross!=-1) 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==way[n].id) break;
dist=GeoCalc::EllipsoidDistance(pomN, pomE, way[n].n, way[n].e);
if (dist <= (double)way[n].dia)
{
if (last_way!=way[n].id)
{
last_way=way[n].id;
switch (way[n].id)
{
case 1:
TL(way[n].azimut);
break;
case 2:
TL(80);
break;
};
}
break;
}
} while(++n<POINTS);
}
*/
void turnL(unsigned char angle, signed char speed) // otoci robota o zadany uhel (otaceni v levo)
{
int azimut;
go(M1, 0);
go(M2, speed);
azimut=read_azimut_mag();
// if((azimut-angle)>192) {azimut+=64; angle+=64};
while(read_azimut_mag() >= (azimut - angle)) usleep(10000);
go(M1, 0);
go(M2, 0);
}
void turnR(unsigned char angle, signed char speed) // otoci robota o zadany uhel (otaceni v pravo)
{
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);
}
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=-1;
command=0;
filtr=0;
done=0;
korekce=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 'o': // forward
start=1;
command=0;
break;
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
turnL(20, 120); // otoci robota o zadany uhel (otaceni v levo)
// 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)
if(start==1)
{
go(M1, 127);
go(M2, 127);
sleep(10);
turnL(5, 120); // otoci robota o zadany uhel (otaceni v levo)
go(M1, 127);
go(M2, 127);
sleep(10);
turnL(5, 120); // otoci robota o zadany uhel (otaceni v levo)
go(M1, 127);
go(M2, 127);
sleep(5);
// turnL(5, 120); // otoci robota o zadany uhel (otaceni v levo)
go(M1, 127);
go(M2, 127);
sleep(10);
start=0;
};
// FindNearestCrossing();
//!!! FindWay();
vzdalenost=echo(US3_Addr);
usleep(30000);
lus=echo(US1_Addr);
usleep(30000);
rus=echo(US2_Addr);
usleep(30000);
if ((vzdalenost<30)||(vzdalenost>150))
{
go(M1, 0); // zastav, neni videt doprovod
go(M2, 0);
break;
};
if ((vzdalenost>60)&&(vzdalenost<100))
{
//FindNearestCrossing();
//if(korekce==0)
{
go(M1, 40);
go(M2, 60);
break;
};
// filtr=0;
};
if ((vzdalenost>=100))
{
filtr=0;
go(M1, 60);
go(M2, 40);
break;
}
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,"US3: %u cm - ",vzdalenost);
pthread_mutex_lock(&mutex);
fprintf(pRouraO,"%fN %fE - ", nord, east);
fprintf(pRouraO,"Bod:%d - ", bod);
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());
fprintf(pRouraO,"US1: %u cm - US2 %u cm\n",lus,rus);
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)
{
vzdalenost=echo(US3_Addr);
usleep(20000);
left=echo(US1_Addr);
usleep(20000);
right=echo(US1_Addr);
usleep(20000);
}
}
*/