Rev Author Line No. Line
1045 jacho 1 //Nataceni za Sluncem
2 //(c) Jan Chroust 2013
3  
4  
5 #include <main.h>
6  
7 #define PIN_SDA PIN_C4
8 #define PIN_SCL PIN_C3
9 #use i2c(master, sda=PIN_SDA, scl=PIN_SCL)
10 #use rs232(baud=9600,parity=N,xmit=PIN_C7,rcv=PIN_C6,bits=8) //rcv TXD xmit RXD
11 #include <math.h>
12 #include <stdio.h>
13  
14 #include <stdlib.h>
15 #include <input.c>
16 #include <string.h>
17  
18 //Akcelerometr
19 #define AK_W 0x38 //adresa akcelerometru zápis
20 #define AK_R 0x39 //adresa akcelerometru ètení
21 #define AK_XH 0x01 //osa X LSB
22 #define AK_XL 0x02 //osa X MSB
23 #define AK_YH 0x03 //osa Y LSB
24 #define AK_YL 0x04 //osa Y MSB
25 #define AK_ZH 0x05 //osa Z LSB
26 #define AK_ZL 0x06 //osa Z MSB
27  
28 //Magnetometr
29 #define MAG_W 0x3C //adresa akcelerometru zápis
30 #define MAG_R 0x3D //adresa akcelerometru ètení
31 #define MAG_XH 0x03 //osa X LSB
32 #define MAG_XL 0x04 //osa X MSB
33 #define MAG_ZH 0x05 //osa Y LSB
34 #define MAG_ZL 0x06 //osa Y MSB
35 #define MAG_YH 0x07 //osa Z LSB
36 #define MAG_YL 0x08 //osa Z MSB
37  
38 //pripojeni motoru
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 - cerveny 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); //nastavi aktivni stav
61  
62 i2c_stop();
63 }
64  
65 void setmag (void)
66 {
67 i2c_start();
68 I2C_Write(MAG_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);
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);
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 namereny %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 (elevace) solarniho panelu
298 {
299 switch(H){
300 case 1: //reverzní chod
301 output_low (AIN2);
302 output_high (AIN1);
303 break;
304  
305 case 2: //dopredu
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 druheho motoru - nastveni nazimutu solarniho panelu
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 (elevace) panelu
336 {
337 //printf("Akcelerometr4: \r\n",);
338 float a;
339 int16 b,c;
340  
341 a=elevace();
342 b= (int16) a;
343 c=abs(H-b);
344  
345 while(c>2) //maximalni odchylka uhlu, aby nebylo potreba panelem hybat
346 {
347 while(H!=b) //probiha dokud se uhel panelu nerovna zadanemu na cele stupne
348 {
349  
350 if(H>b)
351 {
352 motorA(2);
353  
354 }
355 else
356 {
357 motorA(1);
358 }
359  
360 delay_ms (50); //cas sepnuti motoru
361  
362 motorA(3); //vypne motor
363 delay_ms (50); //doma na ustaleni panelu pred merenim
364 a=elevace();
365 b= (int16) a;
366  
367  
368 c=abs(H-b);
369 }
370 }
371 motorA(3); //vypne motor
372 printf("Podarene nastaveni vysky: %Ld\r\n", b);
373  
374  
375 }
376  
377  
378  
379 void azimut_set (int16 H) //slouzi pro nastaveni azimutu panelu
380 {
381 float a;
382 int16 b,c;
383  
384  
385 a=azimut();
386 b= (int16) a;
387 c=abs(H-b);
388  
389 while(c>2) //maximalni odchylka uhlu, aby nebylo potreba panelem hybat
390 {
391 while(H!=b) //probiha dokud se uhel panelu nerovna zadanemu na cele stupne
392 {
393  
394 if(H>b)
395 {
396 motorB(2);
397  
398 }
399 else
400 {
401 motorB(1);
402 }
403  
404 delay_ms (50); //cas sepnuti motoru
405  
406 motorB(3); //vypne motor
407 delay_ms (50); //doma na ustaleni panelu pred merenim
408 a=azimut();
409 b= (int16) a;
410  
411 c=abs(H-b);
412 }
413 }
414 motorA(3); //vypne motor
415 printf("Podarene nastaveni azimutu: %Ld\r\n", b);
416  
417  
418  
419 }
420  
421 void main()
422 {
423 setup_adc_ports(NO_ANALOGS|VSS_VDD);
424 setup_adc(ADC_CLOCK_DIV_2);
425 setup_spi(SPI_SS_DISABLED);
426 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
427 setup_timer_1(T1_DISABLED);
428 setup_timer_2(T2_DISABLED,0,1);
429 setup_ccp1(CCP_OFF);
430 setup_comparator(NC_NC_NC_NC);
431  
432  
433 motorA(3); //vyponuti motoru
434 motorB(3);
435  
436 setAK(); //nastaveni akcelerometru
437 setmag(); //nastveni magnetometru
438 printf("System nataceni panelu \r\n",);
439  
440 while(TRUE)
441 {
442 int i[10];
443 int16 vysledek;
444 int b=0;
445 int c;
446  
447  
448 //nacteni hodnoty elevace ze seriove linky
449 do {
450 b++;
451 i[b] = getchar();
452  
453 // printf("Zadana elevace %d \r\n", i[b]);
454  
455 } while (i[b] != ' ');
456 b=b-1;
457  
458 switch(b){
459 case 1:
460 elevaceZAD=i[1]-48;
461 break;
462  
463 case 2:
464 elevaceZAD=(i[2]-48)+(i[1]-48)*10;
465 break;
466 case 3:
467 elevaceZAD=(i[3]-48)+(i[2]-48)*10+((int16)i[1]-48)*100;
468 break;
469  
470 default:
471  
472 }
473 printf("Zadaná elevace %Ld \r\n", elevaceZAD);
474  
475 i=0;
476 vysledek=0;
477 b=0;
478 c=0;
479  
480 //nacteni hodnoty azimutu ze seriove linky
481 do {
482 b++;
483 i[b] = getchar();
484  
485 //printf("Zadany azimut %d \r\n", i[b]);
486  
487 } while (i[b] != ' ');
488 b=b-1;
489  
490 switch(b){
491 case 1:
492 azimutZAD=i[1]-48;
493 break;
494  
495 case 2:
496 azimutZAD=(i[2]-48)+(i[1]-48)*10;
497 break;
498 case 3:
499 azimutZAD=(i[3]-48)+(i[2]-48)*10+((int16)i[1]-48)*100;
500 break;
501  
502 default:
503  
504 }
505  
506  
507 printf("Zadany azimut %Ld \r\n", azimutZAD);
508  
509  
510 elevace_set (elevaceZAD); //pro nastveni nove elevace solarniho panelu
511 azimut_set(azimutZAD); //pro nastaveni noveho azimutu solarniho panelu
512  
513 delay_ms (2000);
514  
515 }
516 }