Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

Rev Author Line No. Line
365 kakl 1 
/*****************************************************************************/
2 
/*
3 
 *  vector.cpp - Control program for Vector robot
4 
 *
5 
 *      Copyright (C) 2007 KAKL
6 
 *
7 
 *      This program is free software; you can redistribute it and/or modify
8 
 *      it under the terms of the GNU General Public License as published by
9 
 *      the Free Software Foundation; either version 2 of the License, or
10 
 *      (at your option) any later version.
11 
 *
12 
 *      This program is distributed in the hope that it will be useful,
13 
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
14 
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 
 *      GNU General Public License for more details.
16 
 *
17 
 */
18 
/*****************************************************************************/
19 
 
20 
#include <iostream>
21 
#include <getopt.h>
22 
#include <errno.h>
23 
#include <string.h>
24 
#include <pthread.h>
25 
#include <stdio.h>
26 
#include <stdlib.h>
27 
#include <unistd.h>
28 
#include "linux/i2c-dev.h"
29 
#include "linux/i2c.h"
30 
#include <sys/ioctl.h>
31 
#include <sys/types.h>
32 
#include <sys/stat.h>
33 
#include <fcntl.h>
34 
#include "geocalc.h"
35 
 
36 
using namespace std;
37 
 
38 
#define CMPS03_SOFTWARE_REVISION 0x0
39 
#define SRF02_SOFTWARE_REVISION 0x0
40 
 
41 
#define BC_Addr	    0x0B
42 
#define US_Addr	    0x70 // 0xE0 in fact
43 
#define PIC_Addr	0x50 // 0xA0 in fact
44 
 
45 
char vystup[30];
46 
pthread_t thread_1, thread_2;
47 
FILE *pRouraO,*pRouraI;
48 
unsigned int vzdalenost;
49 
char command,ble;
50 
int file;
51 
 
52 
void *print_tele(void *unused);
53 
void *sensors(void *unused);
54 
 
55 
void I2C_addr (int Addr)
56 
{
57 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
58 
    {
59 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
60 
        exit(-5);
61 
    }
62 
}
63 
 
64 
int main(int argc, char *argv[], char *envp[])
65 
{
66 
	fprintf(stdout, "**** Vector Control Programm ****\n");
67 
 
68 
	file = open("/dev/i2c-0", O_RDWR);
69 
	if (file < 0)
70 
	{
71 
		cerr << "Could not open /dev/i2c-0." << endl;
72 
		return -1;
73 
	}
74 
 
75 
    pthread_create(&thread_1, NULL, print_tele, NULL);
76 
//    pthread_create(&thread_2, NULL, sensors, NULL);
77 
 
78 
    while(true)
79 
    {
80 
        int n;
81 
        char Buf[64];
82 
 
83 
        for(n=-127; n<=127;n++)
84 
        {
85 
 
86 
        I2C_addr(US_Addr);
87 
        Buf[0]=0x0;
88 
        Buf[1]=0x51;
89 
        write(file, Buf, 2);
90 
        usleep(80000);
91 
        read(file, Buf, 3);
92 
        vzdalenost=(Buf[1]*256+Buf[2]);
93 
printf("+%X",Buf[2]);
94 
        usleep(300000);
95 
/*
96 
            Buf[0]=(char)n;
97 
            I2C_addr(PIC_Addr);
98 
            Buf[0]=command;
99 
            write(file, Buf, 1);
100 
read(file, Buf, 1);
101 
printf("*%X",Buf[0]);
102 
            usleep(300000);
103 
*/
104 
        };
105 
/*
106 
        for(n=127; n>=-127;n--)
107 
        {
108 
            Buf[0]=(char)n;
109 
            I2C_addr(PIC_Addr);
110 
            Buf[0]=command;
111 
            write(file, Buf, 1);
112 
            usleep(300000);
113 
        };
114 
*/
115 
    };
116 
	close(file);
117 
    pthread_join(thread_1, NULL);
118 
    pthread_join(thread_2, NULL);
119 
 
120 
	return 0;
121 
}
122 
 
123 
void *print_tele(void *unused)
124 
{
125 
    while(true)
126 
    {
127 
        pRouraI = fopen("/home/ble/pipe","r");
128 
        command=fgetc(pRouraI);
129 
        fclose(pRouraI);
130 
        pRouraO = fopen("/home/ble/pipe","w");
131 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %x\n",vzdalenost,ble);
132 
   	printf("Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
133 
	printf("Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
134 
        fclose(pRouraO);
135 
    }
136 
}
137 
 
138 
void *sensors(void *unused)
139 
{
140 
	char Buf[64];
141 
 
142 
    while(true)
143 
    {
144 
        I2C_addr(US_Addr);
145 
        Buf[0]=0x0;
146 
        Buf[1]=0x51;
147 
        write(file, Buf, 2);
148 
        usleep(80000);
149 
        read(file, Buf, 3);
150 
        vzdalenost=(Buf[1]*256+Buf[2]);
151 
        usleep(300000);
152 
 
153 
        I2C_addr(PIC_Addr);
154 
        Buf[0]=command;
155 
        write(file, Buf, 1);
156 
        read(file, Buf, 1);
157 
        ble=Buf[0];
158 
    }
159 
}