Subversion Repositories svnkaklik

/*****************************************************************************/

/*

 *  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;

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);

//    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);

            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, -speed);

    go(M2, speed);

    azimut=read_azimut_mag();

    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 '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)

                FindNearestCrossing();

//!!!                FindWay();

                vzdalenost=echo(US3_Addr);

                usleep(30000);

                lus=echo(US1_Addr);

                usleep(30000);

                rus=echo(US2_Addr);

                usleep(30000);

                if ((vzdalenost>60)&&(vzdalenost<80))

                {

//FindNearestCrossing();

//                    if(korekce==0)

                    {

                        go(M1, 80);

                        go(M2, 80);

                    };

                    korekce=0;

                    filtr=0;

                    break;

                };

                if ((vzdalenost>30)&&(vzdalenost<130))

                {

/*

                    if ((lus<70)&&(lus>30)&&(vzdalenost>60)) // prekazka vlevo

                    {

                        go(M1, 60);

                        go(M2, 40);

                        break;

                    };

*/

/*

                    if ((rus<40)&&(rus>30)) // prekazka vpravo

                    {

                        go(M1, 40);

                        go(M2, 60);

                    };

*/

                    filtr=0;

/*

                    if (vzdalenost<40)

                    {

                        go(M1, 40);

                        go(M2, 60);

                        break;

                    }

                    if (vzdalenost>100)

                    {

                        go(M1, 60);

                        go(M2, 40);

                        break;

                    }

*/

                    if (vzdalenost<55)

                    {

                        go(M1, 40);

                        go(M2, 60);

                    }

                    else

                    {

                        go(M1, 60);

                        go(M2, 40);

                    }

                }

                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, &param);

        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);

    }

}

*/