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 CMPS03_SOFTWARE_REVISION 0x0

#define SRF02_SOFTWARE_REVISION 0x0

#define BC_Addr	    0x0B

#define US3_Addr	    0x70 // 0xE0 in fact; Sonar na doprovod

#define M1	0x50 // 0xA0 in fact

#define M2	0x51 // 0xA2 in fact

char vystup[50];

pthread_t thread_1, thread_2, thread_3;

FILE *pRouraO,*pRouraI;

unsigned int vzdalenost;

char command,ble;

int file;

double n, e;

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)

{

    char Buf[1];

    I2C_addr (Addr);

    Buf[0]=speed;

    write(file, Buf, 1);

}

unsigned int echo(int Addr)

{

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

}

void FindNearestCrossing(void)

{

    int n;

    n=0;

    do

    {

        if (0==cros[n].id) break;

        if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)

        {

            #include "nav.h"

            break;

        }

    } while(++n<POINTS);

}

int main(int argc, char *argv[], char *envp[])

{

    int filtr;

	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_3, NULL, gps, NULL);

//    pthread_create(&thread_2, NULL, sensors, NULL);

    command=0;

    filtr=0;

    while(true)

    {

        switch (command)

        {

            case 'f':   // forward

                go(M1, 70);

                go(M2, 70);

                command=0;

                break;

            case 'b':   // backward

                go(M1, -70);

                go(M2, -70);

                command=0;

                break;

            case 'v':   // stop

                go(M1, 0);

                go(M2, 0);

                command=0;

                break;

            case 's':   // stop

                go(M1, 1);

                go(M2, 1);

                command=0;

                break;

            case 'g':

usleep(180000);

                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, 1);  // zastav, neni videt doprovod

                        go(M2, 1);

                        filtr=6;

                    }

                };

                FindNearestCrossing();

                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 string[2];

    while(true)

    {

        pRouraI = fopen("/home/ble/pipe","r");

        command=fgetc(pRouraI);

        string[0]=command;

        string[1]=0;

        fclose(pRouraI);

        pRouraO = fopen("/home/ble/pipe","w");

        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);

    pthread_mutex_lock(&mutex);

   	fprintf(pRouraO,"%f N %f E\n", n, e);

   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));

	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));

    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

        n=pomN; e=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];

    }

}

*/