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"

using namespace std;

#define CMPS03_SOFTWARE_REVISION 0x0
#define SRF02_SOFTWARE_REVISION 0x0

#define BC_Addr     0x0B
#define US_Addr     0x70 // 0xE0 in fact
#define M1_Addr 0x50 // 0xA0 in fact
#define M2_Addr 0x51 // 0xA2 in fact

char vystup[30];
pthread_t thread_1, thread_2;
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
char command,ble;
int file;

void *print_tele(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);
    }
}

int main(int argc, char *argv[], char *envp[])
{
        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_2, NULL, sensors, NULL);

    char Buf[64];
    command=127;

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

        if (command!=127)
        {
            I2C_addr(M1_Addr);
            Buf[0]=command;
            write(file, Buf, 1);
            I2C_addr(M2_Addr);
            Buf[0]=command;
            write(file, Buf, 1);

            command=127;
        }
    };

        close(file);
    pthread_join(thread_1, NULL);
    pthread_join(thread_2, NULL);

        return 0;
}


void *print_tele(void *unused)
{
    char i;

    while(true)
    {
        pRouraI = fopen("/home/ble/pipe","r");
        i=fgetc(pRouraI);
        switch (i)
        {
            case 'a':
                command=0;
                break;
            case 'b':
                command=20;
                break;
            case 'c':
                command=-20;
                break;
        }

        fclose(pRouraI);
        pRouraO = fopen("/home/ble/pipe","w");
        fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command);
        fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
        fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
        fclose(pRouraO);
    }
}

/*
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];
    }
}
*/