Rev Author Line No. Line
503 kaklik 1 #include "tank.h"
2  
3 #define DEBUG
4  
5 #define TXo PIN_A3 // To the transmitter modulator
6 #include "AX25.c" // podprogram pro prenos telemetrie
7  
8 unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
9 unsigned int8 line; // na ktere strane byla detekovana cara
10 unsigned int8 speed; // rychlost zataceni
11 unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku
12 unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo
13 unsigned int8 movement; // obsahuje aktualni smer zataceni
14 unsigned int8 dira; // pocita dobu po kterou je ztracena cara
15  
16 // Konstanty pro dynamiku pohybu
17 #define T_DIRA 120 // po jakem case zataceni se detekuje dira
18 #define INC_SPEED 1 // prirustek rychlosti v jednom kroku
19 #define FW_POMALU 230 // trochu mimo caru vnitrni pas
20 #define FW_ZATACKA 240 // rychlost vnejsiho kola pri zataceni
21 #define FW_STREDNE 240 // trochu mimo caru vnejsi pas
22 #define COUVANI 750 // couvnuti zpet na caru, po detekci diry
23 #define PRES_DIRU 300
24 #define MAX_ROVINKA (255-FW_STREDNE)
25 #define TRESHOLD 6 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd
26 #define BUMPER_TRESHOLD 128
27 #define CIK_CAK 30000
28 #define T_CIHLA 50 // perioda detekce cihly
29  
30 //motory //Napred vypnout potom zapnout!
31 #define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred
32 #define FL output_low(PIN_B7); output_high(PIN_B6)
33 #define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad
34 #define BL output_low(PIN_B6); output_high(PIN_B7)
35 #define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav
36 #define STOPL output_low(PIN_B6);output_low(PIN_B7)
37  
38 #define L 0b10 // left
39 #define R 0b01 // right
40 #define S 0b11 // straight
41  
42 //cidla
43 #define RSENSOR C2OUT // Senzory na caru
44 #define LSENSOR C1OUT
45 #define BUMPER PIN_A4 // Senzor na cihlu
46  
47 #define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod
48 #define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod
49  
50 #DEFINE SOUND_HI PIN_A6 // komplementarni vystupy pro piezo pipak
51 #DEFINE SOUND_LO PIN_A7
52  
53 char AXstring[40]; // Buffer pro prenos telemetrie
54  
55 // makro pro PWM
56 #define GO(motor, direction, power) if(get_timer0()<=power) \
57 {direction##motor;} else {stop##motor;}
58  
59 #int_TIMER2
60 void TIMER2_isr()
61 {
62 if (speed<255) speed+=INC_SPEED;
63 if (rovinka<MAX_ROVINKA) rovinka++;
64 if (dira<=T_DIRA) dira++;
65 }
66 // Primitivni Pipani
67 void beep(unsigned int16 period, unsigned int16 length)
68 {
69 unsigned int16 nn;
70  
71 for(nn=length; nn>0; nn--)
72 {
73 output_high(SOUND_HI);output_low(SOUND_LO);
74 delay_us(period);
75 output_high(SOUND_LO);output_low(SOUND_HI);
76 delay_us(period);
77 }
78 }
79 /******************************************************************************/
80 void diagnostika()
81 {
82 unsigned int16 n;
83  
84 while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku
85 {
86 for (n=500; n<800; n+=100)
87 {
88 beep(n,n); //beep UP
89 };
90 Delay_ms(1000);
91 //zastav vse
92 STOPL; STOPR;
93 //pravy pas
94 FR; Delay_ms(1000); STOPR; Delay_ms(1000);
95 BR; Delay_ms(1000); STOPR; Delay_ms(1000);
96 Beep(880,100); Delay_ms(1000);
97 //levy pas
98 FL; Delay_ms(1000); STOPL; Delay_ms(1000);
99 BL; Delay_ms(1000); STOPL; Delay_ms(1000);
100 Beep(880,100); Delay_ms(1000);
101 //oba pasy
102 FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
103 BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
104 };
105 while (input(DIAG_SENSORS)) // spusteni diagnostiky cidel
106 {
107 if (RSENSOR) beep(900,500);
108 if (LSENSOR) beep(800,500);
109 if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(1000,500);
110 };
111 }
112 ///////////////////////////////////////////////////////////////////////////////
113 void cikcak()
114 {
115 unsigned int16 n;
116 sem1:
117 n=CIK_CAK;
118 while (0==RSENSOR||LSENSOR) // zkontroluj caru
119 {
120 if (n==CIK_CAK) // zmen smer zataceni
121 {
122 n=0;
123 switch(movement)
124 {
125 case L:
126 FL;BR;
127 movement=R;
128 break;
129 case R:
130 FR;BL;
131 movement=L;
132 break;
133 case S:
134 FL;BR;
135 movement=R;
136 n=CIK_CAK/2;
137 break;
138 }
139 }
140 n++;
141 }
142 STOPL;STOPR;
143 line = RSENSOR; // cteni senzoru na caru
144 line |= LSENSOR << 1;
145 if (line==0) goto sem1;
146 // nasli jsme caru
147 line=S;
148 }
149 ///////////////////////////////////////////////////////////////////////////////
150 void objizdka() // objede cihlu
151 {
152 unsigned int16 n;
153  
154 BL;BR;Delay_ms(150);
155 STOPR;STOPL;
156 beep(900,1000);
157 // movement=S;
158 // cikcak();
159  
160 BR; FL; Delay_ms(270); // otoc se 70° do prava
161  
162 FR; FL; Delay_ms(500); // popojed rovne
163  
164 BL; Delay_ms(30); // otoc se 90° do leva
165 STOPL; FR; Delay_ms(500);
166  
167 FR; FL; Delay_ms(100); // popojed rovne na slepo
168 for(n=40000;n>0;n--) // popojed rovne ale kontroluj caru
169 {
170 line = RSENSOR; // cteni senzoru na caru
171 line |= LSENSOR << 1;
172 if (line!=0) {Delay_ms(150); break;}
173 // Delay_ms(1);
174 }
175  
176 BR; FL; // otoc se 60° do prava
177 for(n=40000;n>0;n--)
178 {
179 line = RSENSOR; // cteni senzoru na caru
180 line |= LSENSOR << 1;
181 if (line!=0) break;
182 // Delay_ms(1);
183 }
184 STOPR; STOPL;
185  
186 movement=L; //R;
187 cikcak();
188 dira=0;
189 }
190 ///////////////////////////////////////////////////////////////////////////////
191 void prejeddiru() // vyresi diru
192 {
193 unsigned int16 n;
194 unsigned int8 speed_dira;
195  
196 STOPL;STOPR;
197 speed_dira=speed;
198 beep(1000,500);
199 switch (movement) //vrat se zpet na caru
200 {
201 case L:
202 for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);}
203 STOPL;STOPR;
204 break;
205 case R:
206 for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);}
207 STOPL;STOPR;
208 break;
209 case S:
210 goto sem;
211 break;
212 }
213 beep(1000,500);
214  
215 /*line=0;
216 FR; BL; Delay_ms(400); // otoc se na caru
217 beep(1000,500);
218 while(line==0)
219 {
220 line = RSENSOR; // cteni senzoru na caru
221 line |= LSENSOR << 1;
222 }
223 FL;BR; Delay_ms(60); // zabrzdi
224 STOPL; STOPR;
225  
226 FL; BR; Delay_ms(700); // otacka 180 deg
227 STOPL; STOPR;*/
228  
229 FR;FL; //popojed rovne
230 for(n=PRES_DIRU;n>0;n--)
231 {
232 line = RSENSOR; // cteni senzoru na caru
233 line |= LSENSOR << 1;
234 if (line!=0) break;
235 Delay_ms(1);
236 }
237 sem:
238 STOPL; STOPR;
239 movement=S;
240 cikcak(); // najdi caru
241 dira=0;
242 }
243 ///////////////////////////////////////////////////////////////////////////////
244 void main()
245 {
246 unsigned int16 n; // pro FOR
247  
248 STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi
249  
250 setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator
251  
252 port_b_pullups(TRUE); // pullups pro piano na diagnostiku
253 setup_spi(FALSE);
254 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM
255  
256 setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci
257 // preruseni kazdych 10ms
258 setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik
259 setup_adc(ADC_CLOCK_INTERNAL);
260 set_adc_channel(2);
261 setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik
262 setup_ccp1(CCP_COMPARE_RESET_TIMER);
263 CCP_1=(2^10)-1; // prevod kazdou 1ms
264  
265 setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary
266 setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd
267  
268 Beep(1000,200); //double beep
269 Delay_ms(50);
270 Beep(1000,200);
271 Delay_ms(1000); // 1s
272  
273 // povoleni rizeni rychlosti zataceni pres preruseni
274 enable_interrupts(INT_TIMER2);
275 enable_interrupts(GLOBAL);
276  
277 /*---------------------------------------------------------------------------*/
278 sensors=S;
279 line=S;
280 last=S;
281 movement=S;
282 speed=FW_POMALU;
283  
284 diagnostika();
285 //cikcak(); // toc se, abys nasel caru
286 Delay_ms(500);
287 Beep(1000,200);
288 Delay_ms(500);
289  
290 while(true) // hlavni smycka (jizda podle cary)
291 {
292 sensors = RSENSOR; // cteni senzoru na caru
293 sensors |= LSENSOR << 1;
294  
295 if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (dira<=T_CIHLA)) objizdka();
296  
297 switch (sensors) // zatacej podle toho, kde vidis caru
298 {
299 case S: // rovne
300 FL; FR; // pokud se jede dlouho rovne, tak pridej
301 dira=0;
302 movement=S;
303 continue;
304 case L: // trochu vlevo
305 GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
306 line=L;
307 dira=0;
308 movement=L;
309 continue;
310 case R: // trochu vpravo
311 GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
312 line=R;
313 dira=0;
314 movement=R;
315 continue;
316 default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal
317 }
318 rovinka=0;
319 if (dira>=T_DIRA) prejeddiru();
320 if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
321 {
322 last=line;
323 speed=FW_ZATACKA;
324 }
325 if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru
326 {
327 STOPL;
328 GO(R, F, speed);
329 movement=L;
330 }
331 else
332 {
333 STOPR;
334 GO(L, F, speed);
335 movement=R;
336 }
337 } // while(true)
338 }
339