Subversion Repositories svnkaklik

Rev

Rev 365 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 365 Rev 366
Line 38... Line 38...
38 
#define CMPS03_SOFTWARE_REVISION 0x0
 38 
#define CMPS03_SOFTWARE_REVISION 0x0
39 
#define SRF02_SOFTWARE_REVISION 0x0
 39 
#define SRF02_SOFTWARE_REVISION 0x0
40 
 
 40 
 
41 
#define BC_Addr	    0x0B
 41 
#define BC_Addr	    0x0B
42 
#define US_Addr	    0x70 // 0xE0 in fact
 42 
#define US_Addr	    0x70 // 0xE0 in fact
43 
#define PIC_Addr	0x50 // 0xA0 in fact
 43 
#define M1_Addr	0x50 // 0xA0 in fact
- 
 
 44 
#define M2_Addr	0x51 // 0xA2 in fact
44 
 
 45 
 
45 
char vystup[30];
 46 
char vystup[30];
46 
pthread_t thread_1, thread_2;
 47 
pthread_t thread_1, thread_2;
47 
FILE *pRouraO,*pRouraI;
 48 
FILE *pRouraO,*pRouraI;
48 
unsigned int vzdalenost;
 49 
unsigned int vzdalenost;
49 
char command,ble;
 50 
char command,ble;
50 
int file;
 51 
int file;
51 
 
 52 
 
52 
void *print_tele(void *unused);
 53 
void *print_tele(void *unused);
53 
void *sensors(void *unused);
 54 
//void *sensors(void *unused);
54 
 
 55 
 
55 
void I2C_addr (int Addr)
 56 
void I2C_addr (int Addr)
56 
{
 57 
{
57 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
 58 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
58 
    {
 59 
    {
Line 73... Line 74...
73 
	}
 74 
	}
74 
 
 75 
 
75 
    pthread_create(&thread_1, NULL, print_tele, NULL);
 76 
    pthread_create(&thread_1, NULL, print_tele, NULL);
76 
//    pthread_create(&thread_2, NULL, sensors, NULL);
 77 
//    pthread_create(&thread_2, NULL, sensors, NULL);
77 
 
 78 
 
- 
 
 79 
    char Buf[64];
- 
 
 80 
    command=127;
- 
 
 81 
 
78 
    while(true)
 82 
    while(true)
79 
    {
 83 
    {
80 
        int n;
 - 
 
81 
        char Buf[64];
 - 
 
82 
 
 - 
 
83 
        for(n=-127; n<=127;n++)
 - 
 
84 
        {
 - 
 
85 
 
 - 
 
86 
        I2C_addr(US_Addr);
 84 
        I2C_addr(US_Addr);
87 
        Buf[0]=0x0;
 85 
        Buf[0]=0x0;
88 
        Buf[1]=0x51;
 86 
        Buf[1]=0x51;
89 
        write(file, Buf, 2);
 87 
        write(file, Buf, 2);
90 
        usleep(80000);
 88 
        usleep(80000);
91 
        read(file, Buf, 3);
 89 
        read(file, Buf, 3);
92 
        vzdalenost=(Buf[1]*256+Buf[2]);
 90 
        vzdalenost=(Buf[1]*256+Buf[2]);
93 
printf("+%X",Buf[2]);
 - 
 
94 
        usleep(300000);
 91 
        usleep(300000);
95 
/*
 92 
 
96 
            Buf[0]=(char)n;
 93 
        if (command!=127)
- 
 
 94 
        {
97 
            I2C_addr(PIC_Addr);
 95 
            I2C_addr(M1_Addr);
98 
            Buf[0]=command;
 96 
            Buf[0]=command;
99 
            write(file, Buf, 1);
 97 
            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);
 98 
            I2C_addr(M2_Addr);
110 
            Buf[0]=command;
 99 
            Buf[0]=command;
111 
            write(file, Buf, 1);
 100 
            write(file, Buf, 1);
- 
 
 101 
 
112 
            usleep(300000);
 102 
            command=127;
113 
        };
 103 
        }
114 
*/
 - 
 
115 
    };
 104 
    };
- 
 
 105 
 
116 
	close(file);
 106 
	close(file);
117 
    pthread_join(thread_1, NULL);
 107 
    pthread_join(thread_1, NULL);
118 
    pthread_join(thread_2, NULL);
 108 
    pthread_join(thread_2, NULL);
119 
 
 109 
 
120 
	return 0;
 110 
	return 0;
121 
}
 111 
}
122 
 
 112 
 
- 
 
 113 
 
123 
void *print_tele(void *unused)
 114 
void *print_tele(void *unused)
124 
{
 115 
{
- 
 
 116 
    char i;
- 
 
 117 
 
125 
    while(true)
 118 
    while(true)
126 
    {
 119 
    {
127 
        pRouraI = fopen("/home/ble/pipe","r");
 120 
        pRouraI = fopen("/home/ble/pipe","r");
128 
        command=fgetc(pRouraI);
 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 
 
129 
        fclose(pRouraI);
 135 
        fclose(pRouraI);
130 
        pRouraO = fopen("/home/ble/pipe","w");
 136 
        pRouraO = fopen("/home/ble/pipe","w");
131 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %x\n",vzdalenost,ble);
 137 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command);
132 
   	printf("Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
 138 
   	fprintf(pRouraO,"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));
 139 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
134 
        fclose(pRouraO);
 140 
        fclose(pRouraO);
135 
    }
 141 
    }
136 
}
 142 
}
137 
 
 143 
 
- 
 
 144 
/*
138 
void *sensors(void *unused)
 145 
void *sensors(void *unused)
139 
{
 146 
{
140 
	char Buf[64];
 147 
	char Buf[64];
141 
 
 148 
 
142 
    while(true)
 149 
    while(true)
Line 155... Line 162...
155 
        write(file, Buf, 1);
 162 
        write(file, Buf, 1);
156 
        read(file, Buf, 1);
 163 
        read(file, Buf, 1);
157 
        ble=Buf[0];
 164 
        ble=Buf[0];
158 
    }
 165 
    }
159 
}
 166 
}
- 
 
 167 
*/