Subversion Repositories svnkaklik

Rev

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
366 kakl 43 
#define M1_Addr	0x50 // 0xA0 in fact
44 
#define M2_Addr	0x51 // 0xA2 in fact
365 kakl 45 
 
46 
char vystup[30];
47 
pthread_t thread_1, thread_2;
48 
FILE *pRouraO,*pRouraI;
49 
unsigned int vzdalenost;
50 
char command,ble;
51 
int file;
52 
 
53 
void *print_tele(void *unused);
366 kakl 54 
//void *sensors(void *unused);
365 kakl 55 
 
56 
void I2C_addr (int Addr)
57 
{
58 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
59 
    {
60 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
61 
        exit(-5);
62 
    }
63 
}
64 
 
65 
int main(int argc, char *argv[], char *envp[])
66 
{
67 
	fprintf(stdout, "**** Vector Control Programm ****\n");
68 
 
69 
	file = open("/dev/i2c-0", O_RDWR);
70 
	if (file < 0)
71 
	{
72 
		cerr << "Could not open /dev/i2c-0." << endl;
73 
		return -1;
74 
	}
75 
 
76 
    pthread_create(&thread_1, NULL, print_tele, NULL);
77 
//    pthread_create(&thread_2, NULL, sensors, NULL);
78 
 
366 kakl 79 
    char Buf[64];
80 
    command=127;
81 
 
365 kakl 82 
    while(true)
83 
    {
84 
        I2C_addr(US_Addr);
85 
        Buf[0]=0x0;
86 
        Buf[1]=0x51;
87 
        write(file, Buf, 2);
88 
        usleep(80000);
89 
        read(file, Buf, 3);
90 
        vzdalenost=(Buf[1]*256+Buf[2]);
91 
        usleep(300000);
366 kakl 92 
 
93 
        if (command!=127)
94 
        {
95 
            I2C_addr(M1_Addr);
365 kakl 96 
            Buf[0]=command;
97 
            write(file, Buf, 1);
366 kakl 98 
            I2C_addr(M2_Addr);
365 kakl 99 
            Buf[0]=command;
100 
            write(file, Buf, 1);
366 kakl 101 
 
102 
            command=127;
103 
        }
365 kakl 104 
    };
366 kakl 105 
 
365 kakl 106 
	close(file);
107 
    pthread_join(thread_1, NULL);
108 
    pthread_join(thread_2, NULL);
109 
 
110 
	return 0;
111 
}
112 
 
366 kakl 113 
 
365 kakl 114 
void *print_tele(void *unused)
115 
{
366 kakl 116 
    char i;
117 
 
365 kakl 118 
    while(true)
119 
    {
120 
        pRouraI = fopen("/home/ble/pipe","r");
366 kakl 121 
        i=fgetc(pRouraI);
122 
        switch (i)
123 
        {
124 
            case 'a':
125 
                command=0;
126 
                break;
127 
            case 'b':
128 
                command=20;
129 
                break;
130 
            case 'c':
131 
                command=-20;
132 
                break;
133 
        }
134 
 
365 kakl 135 
        fclose(pRouraI);
136 
        pRouraO = fopen("/home/ble/pipe","w");
366 kakl 137 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command);
138 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
139 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
365 kakl 140 
        fclose(pRouraO);
141 
    }
142 
}
143 
 
366 kakl 144 
/*
365 kakl 145 
void *sensors(void *unused)
146 
{
147 
	char Buf[64];
148 
 
149 
    while(true)
150 
    {
151 
        I2C_addr(US_Addr);
152 
        Buf[0]=0x0;
153 
        Buf[1]=0x51;
154 
        write(file, Buf, 2);
155 
        usleep(80000);
156 
        read(file, Buf, 3);
157 
        vzdalenost=(Buf[1]*256+Buf[2]);
158 
        usleep(300000);
159 
 
160 
        I2C_addr(PIC_Addr);
161 
        Buf[0]=command;
162 
        write(file, Buf, 1);
163 
        read(file, Buf, 1);
164 
        ble=Buf[0];
165 
    }
166 
}
366 kakl 167 
*/