Rev Author Line No. Line
1045 jacho 1 #include "C:\Users\Honza\Documents\pic\azimut\main.h"
2  
3 #define PIN_SDA PIN_C4
4 #define PIN_SCL PIN_C3
5 #use i2c(master, sda=PIN_SDA, scl=PIN_SCL)
6 #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD
7 #include <math.h>
8 #include <stdio.h>
9 //#include "sscanf.c"
10 #include <stdlib.h>
11 #include <input.c>
12 #include <string.h>
13  
14 //Akcelerometr
15 #define AK_W 0x38 //adresa akcelerometru zápis
16 #define AK_R 0x39 //adresa akcelerometru ètení
17 #define AK_XH 0x01 //osa X LSB
18 #define AK_XL 0x02 //osa X MSB
19 #define AK_YH 0x03 //osa Y LSB
20 #define AK_YL 0x04 //osa Y MSB
21 #define AK_ZH 0x05 //osa Z LSB
22 #define AK_ZL 0x06 //osa Z MSB
23  
24 //Magnetometr
25 #define MAG_W 0x3C //adresa akcelerometru zápis
26 #define MAG_R 0x3D //adresa akcelerometru ètení
27 #define MAG_XH 0x03 //osa X LSB
28 #define MAG_XL 0x04 //osa X MSB
29 #define MAG_ZH 0x05 //osa Y LSB
30 #define MAG_ZL 0x06 //osa Y MSB
31 #define MAG_YH 0x07 //osa Z LSB
32 #define MAG_YL 0x08 //osa Z MSB
33  
34  
35  
36  
37  
38 //pøipojení motorù
39 //AIN1 - pro vysku slunce
40 #define AIN1 PIN_D0
41 #define AIN2 PIN_D1
42 //motor A -cerveny vodic na AOUT1
43 //motor A -modry vodic na Aout2
44  
45 //AIN2 - pro azimut
46 #define BIN1 PIN_D2
47 #define BIN2 PIN_D3
48 //motor B - èerveny vodic na BOUT2
49 //motor B - modrý vodic na BOUT1
50  
51 signed int16 X, Y, Z,AX, AY, AZ; //promenne pro magnetometr a akcelerometr
52  
53 unsigned int16 azimutZAD, elevaceZAD;
54  
55 void setAK (void) //nastaveni akcelerometru
56 {
57 i2c_start();
58 I2C_Write(AK_W);
59 I2C_write(0x2A);
60 I2C_write(0x01); //nastaví aktivní stav
61  
62 i2c_stop();
63 }
64  
65 void setmag (void)
66 {
67 i2c_start();
68 I2C_Write(MAG_W); // W
69 I2C_Write(0x00);
70 I2C_Write(0x78);
71 i2c_stop();
72 Delay_ms(6);
73  
74 i2c_start();
75 I2C_Write(MAG_W); // W
76 I2C_Write(0x01);
77 I2C_Write(0x00);
78 i2c_stop();
79  
80 Delay_ms(6);
81  
82 i2c_start();
83 I2C_Write(MAG_W); // W
84 I2C_Write(0x02);
85 I2C_Write(0x00);
86 i2c_stop();
87 Delay_ms(6);
88 }
89  
90 int16 akR (int8 H, int8 L) //vycitani hodnot z akcelerometru
91 {
92 unsigned int8 XL=0,XH=0;
93 signed int16 x;
94  
95 i2c_start();
96 I2C_Write(AK_W);
97 I2C_write(H);
98 i2c_start();
99 I2C_Write(AK_R);
100 XH=i2c_read(0);
101 i2c_stop();
102  
103 i2c_start();
104 I2C_Write(AK_W);
105 I2C_write(L);
106 i2c_start();
107 I2C_Write(AK_R);
108 XL=i2c_read(0);
109 i2c_stop();
110  
111 x = (((unsigned int16) XH << 8) + XL ); //prevod na 16bit hodnotu
112 x=x/4;
113  
114 return x;
115 }
116  
117 int16 magR (int8 H, int8 L) //vycitani hodnot z magnetometru
118 {
119 unsigned int8 XL=0,XH=0;
120 signed int16 x;
121  
122 i2c_start();
123 I2C_Write(MAG_W);
124 I2C_write(H);
125 i2c_start();
126 I2C_Write(MAG_R);
127 XH=i2c_read(0);
128 i2c_stop();
129  
130 i2c_start();
131 I2C_Write(MAG_W);
132 I2C_write(L);
133 i2c_start();
134 I2C_Write(MAG_R);
135 XL=i2c_read(0);
136 i2c_stop();
137  
138 x = (((unsigned int16) XH << 8) + XL );
139  
140  
141 return x;
142 }
143  
144 float elevace (void) //vypocet aktualni elevace panelu
145 {
146 //printf("Akcelerometr5: \r\n",);
147  
148 X= akR (AK_XH, AK_XL);
149 Y= akR (AK_YH, AK_YL);
150 Z= akR (AK_ZH, AK_ZL);
151  
152 AX=abs(X);
153 AY=abs(Y)+250;
154 AZ=abs(Z)+250;
155  
156 float a, b;
157 a=(float)Y/Z;
158 b=atan(a);
159 b = (b/3.14)*180;
160 b=abs(b);
161  
162 if(((AX>AY) || (AX>AZ))) //indikace prevraceni panelu
163 {
164 printf("Prevracený panel)\r\n", );
165 }
166 else
167 {
168 if(Z==0) //osetreni proti deleni 0
169 {
170 if(Y>0)
171 {
172 b=180;
173 }
174 else
175 {
176 b=0;
177 }
178 }
179 else
180 {
181 if(Z>0)
182 {
183 if(Y>=0)
184 {
185 b=90+b;
186 }
187 else
188 {
189 b=90-b;
190 }
191 }
192 else
193 {
194 if(Y>=0)
195 {
196 b=180-b;
197 }
198 else
199 {
200 b=270+b;
201 }
202 }
203  
204 }
205  
206  
207  
208 }
209 // printf("uhel namìreny %10.2f \r\n", b);
210  
211 if(b>355)
212 {
213 b=0;
214 }
215 else
216 {
217  
218 }
219 return b;
220  
221 }
222  
223 float azimut (void) //vypocet aktualni vysky panelu
224 {
225 X= magR (MAG_XH, MAG_XL);
226 Y= magR (MAG_YH, MAG_YL);
227 Z= magR (MAG_ZH, MAG_ZL);
228  
229  
230  
231  
232 AX=abs(X);
233 AY=abs(Y);
234 AZ=abs(Z);
235  
236 float a, b;
237 a=(float)Y/X;
238 b=atan(a);
239 b = (b/3.14)*180;
240 b=abs(b);
241  
242  
243  
244  
245 if(X==0) //osetreni proti deleni 0
246 {
247 if(Y>0)
248 {
249 b=90;
250 }
251 else
252 {
253 b=270;
254 }
255 }
256 else
257 {
258 if(X>0)
259 {
260 if(Y>=0)
261 {
262 b=b;
263 }
264 else
265 {
266 b=360-b;
267 }
268 }
269 else
270 {
271 if(Y>=0)
272 {
273 b=180-b;
274 }
275 else
276 {
277 b=180+b;
278 }
279 }
280  
281 }
282  
283  
284 if(b>355)
285 {
286 b=0;
287 }
288 else
289 {
290  
291 }
292  
293 return b;
294  
295 }
296  
297 void motorA (int8 H) //pro ovladani prvniho motoru nastaveni vysky
298 {
299 switch(H){
300 case 1: //reverzní chod
301 output_low (AIN2);
302 output_high (AIN1);
303 break;
304  
305 case 2: //dopøedu
306 output_low (AIN1);
307 output_high (AIN2);
308 break;
309  
310 default:
311 output_low (AIN2);
312 output_low (AIN1);
313 }
314 }
315  
316 void motorB (int8 H) //pro ovladani prvniho motoru nastaveni vysky
317 {
318 switch(H){
319 case 1: //reverzní chod
320 output_low (BIN2);
321 output_high (BIN1);
322 break;
323  
324 case 2: //dopøedu
325 output_low (BIN1);
326 output_high (BIN2);
327 break;
328  
329 default:
330 output_low (BIN2);
331 output_low (BIN1);
332 }
333 }
334  
335 void elevace_set (int16 H) //slouzi pro nastaveni nove vysky panelu
336 {
337 //printf("Akcelerometr4: \r\n",);
338 float a;
339 int16 b,c;
340  
341  
342  
343  
344  
345  
346  
347 a=elevace();
348 b= (int16) a;
349 c=abs(H-b);
350  
351 while(c>2) //maximalni odchylka uhlu, aby nebylo potreba panelem hybat
352 {
353 while(H!=b) //probiha dokud se uhel panelu nerovna zadanemu na cele stupne
354 {
355  
356 if(H>b)
357 {
358 motorA(2);
359  
360 }
361 else
362 {
363 motorA(1);
364 }
365  
366 delay_ms (50); //cas sepnuti motoru
367  
368 motorA(3); //vypne motor
369 delay_ms (50); //doma na ustaleni panelu pred merenim
370 a=elevace();
371 b= (int16) a;
372  
373  
374 c=abs(H-b);
375 }
376 }
377 motorA(3); //vypne motor
378 printf("Podaøené nastavení výška: %Ld\r\n", b);
379  
380  
381 }
382  
383  
384  
385 void azimut_set (int16 H) //slouzi pro nastaveni nove vysky panelu
386 {
387 float a;
388 int16 b,c;
389  
390  
391 a=azimut();
392 b= (int16) a;
393 c=abs(H-b);
394  
395 while(c>2) //maximalni odchylka uhlu, aby nebylo potreba panelem hybat
396 {
397 while(H!=b) //probiha dokud se uhel panelu nerovna zadanemu na cele stupne
398 {
399  
400 if(H>b)
401 {
402 motorB(2);
403  
404 }
405 else
406 {
407 motorB(1);
408 }
409  
410 delay_ms (50); //cas sepnuti motoru
411  
412 motorB(3); //vypne motor
413 delay_ms (50); //doma na ustaleni panelu pred merenim
414 a=azimut();
415 b= (int16) a;
416  
417 c=abs(H-b);
418 }
419 }
420 motorA(3); //vypne motor
421 printf("Podaøené nastavení azimut: %Ld\r\n", b);
422  
423  
424  
425 }
426  
427 void main()
428 {
429 setup_adc_ports(NO_ANALOGS|VSS_VDD);
430 setup_adc(ADC_CLOCK_DIV_2);
431 setup_spi(SPI_SS_DISABLED);
432 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
433 setup_timer_1(T1_DISABLED);
434 setup_timer_2(T2_DISABLED,0,1);
435 setup_ccp1(CCP_OFF);
436 setup_comparator(NC_NC_NC_NC);// This device COMP currently not supported by the PICWizard
437  
438 setup_adc_ports(PIN_A0); //piny pro A/D RA0
439 setup_adc_ports(PIN_A1); //piny pro A/D RA1
440  
441 motorA(3); //vyponuti motorù
442 motorB(3);
443  
444 setAK(); //nastaveni akcelerometru
445 setmag();
446 printf("System nataceni panelu \r\n",);
447 while(TRUE)
448 {
449 int i[10];
450 int16 vysledek;
451 int b=0;
452 int c;
453 do {
454 b++;
455 i[b] = getchar();
456  
457 // printf("zadaný azimut %d \r\n", i[b]);
458  
459 } while (i[b] != ' ');
460 b=b-1;
461  
462 switch(b){
463 case 1: //reverzní chod
464 elevaceZAD=i[1]-48;
465 break;
466  
467 case 2: //dopøedu
468 elevaceZAD=(i[2]-48)+(i[1]-48)*10;
469 break;
470 case 3: //dopøedu
471 elevaceZAD=(i[3]-48)+(i[2]-48)*10+((int16)i[1]-48)*100;
472 break;
473  
474 default:
475  
476 }
477 printf("Zadaná elevace %Ld \r\n", elevaceZAD);
478  
479 i=0;
480 vysledek=0;
481 b=0;
482 c=0;
483 do {
484 b++;
485 i[b] = getchar();
486  
487 //printf("Zadaný azimut %d \r\n", i[b]);
488  
489 } while (i[b] != ' ');
490 b=b-1;
491  
492 switch(b){
493 case 1: //reverzní chod
494 azimutZAD=i[1]-48;
495 break;
496  
497 case 2: //dopøedu
498 azimutZAD=(i[2]-48)+(i[1]-48)*10;
499 break;
500 case 3: //dopøedu
501 azimutZAD=(i[3]-48)+(i[2]-48)*10+((int16)i[1]-48)*100;
502 break;
503  
504 default:
505  
506 }
507  
508  
509 printf("Zadaný azimut %Ld \r\n", azimutZAD);
510  
511  
512  
513  
514 elevace_set (elevaceZAD);
515 azimut_set(azimutZAD);
516  
517  
518  
519 delay_ms (2000);
520  
521  
522 }
523 }