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;
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, &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);
    }
}
*/