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 87 // 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 200 // rychlost vnejsiho kola pri zataceni
21 #define FW_STREDNE 240 // trochu mimo caru vnejsi pas
22 #define COUVANI 600 // couvnuti zpet na caru, po detekci diry
23 #define PRES_DIRU 250
24 #define MAX_ROVINKA (255-FW_STREDNE)
25 #define TRESHOLD 10 // 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))
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(200);
155 STOPR;STOPL;
156 beep(900,1000);
157 // movement=S;
158 //cikcak();
159  
160 BL; FR; Delay_ms(215); // otoc se 70° do leva
161  
162 FR; FL; Delay_ms(600); // popojed rovne
163  
164 BR; Delay_ms(50); // otoc se 90° do prava
165 STOPR; FL; Delay_ms(700);
166  
167 FR; FL; Delay_ms(100); // popojed rovne na slepo
168 for(n=600;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(50); break;} // kdyz narazis na caru, za chvili zastav
173 Delay_ms(1);
174 }
175  
176 BL; // otoc se 60° do leva
177 for(n=600;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=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(800,500);
214  
215 line=0;
216 FR; BL; Delay_ms(300); // otoc se na caru
217 while(line==0)
218 {
219 line = RSENSOR; // cteni senzoru na caru
220 line |= LSENSOR << 1;
221 }
222 FL;BR; Delay_ms(60);
223 STOPL; STOPR;
224  
225 FL; BR; Delay_ms(500);
226 STOPL; STOPR;
227  
228 Delay_ms(1000);
229  
230 FR;FL; //popojed rovne
231 for(n=PRES_DIRU;n>0;n--)
232 {
233 line = RSENSOR; // cteni senzoru na caru
234 line |= LSENSOR << 1;
235 if (line!=0) break;
236 Delay_ms(1);
237 }
238 sem:
239 STOPL; STOPR;
240 movement=S;
241 cikcak(); // najdi caru
242 dira=0;
243 }
244 ///////////////////////////////////////////////////////////////////////////////
245 void main()
246 {
247 unsigned int16 n; // pro FOR
248  
249 STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi
250  
251 setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator
252  
253 port_b_pullups(TRUE); // pullups pro piano na diagnostiku
254 setup_spi(FALSE);
255 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM
256  
257 setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci
258 // preruseni kazdych 10ms
259 setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik
260 setup_adc(ADC_CLOCK_INTERNAL);
261 set_adc_channel(2);
262 setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik
263 setup_ccp1(CCP_COMPARE_RESET_TIMER);
264 CCP_1=(2^10)-1; // prevod kazdou 1ms
265  
266 setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary
267 setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd
268  
269 Beep(1000,200); //double beep
270 Delay_ms(50);
271 Beep(1000,200);
272 Delay_ms(1000); // 1s
273  
274 // povoleni rizeni rychlosti zataceni pres preruseni
275 enable_interrupts(INT_TIMER2);
276 enable_interrupts(GLOBAL);
277  
278 /*---------------------------------------------------------------------------*/
279 sensors=S;
280 line=S;
281 last=S;
282 movement=S;
283 speed=FW_POMALU;
284  
285 diagnostika();
286 // cikcak(); // toc se, abys nasel caru
287 Delay_ms(500);
288 Beep(1000,200);
289 Delay_ms(500);
290  
291 while(true) // hlavni smycka (jizda podle cary)
292 {
293 sensors = RSENSOR; // cteni senzoru na caru
294 sensors |= LSENSOR << 1;
295  
296 if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (dira<=T_CIHLA)) objizdka();
297  
298 switch (sensors) // zatacej podle toho, kde vidis caru
299 {
300 case S: // rovne
301 FL; FR; // pokud se jede dlouho rovne, tak pridej
302 dira=0;
303 movement=S;
304 continue;
305 case L: // trochu vlevo
306 GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
307 line=L;
308 dira=0;
309 movement=L;
310 continue;
311 case R: // trochu vpravo
312 GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
313 line=R;
314 dira=0;
315 movement=R;
316 continue;
317 default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal
318 }
319 rovinka=0;
320 if (dira>=T_DIRA) prejeddiru();
321 if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
322 {
323 last=line;
324 speed=FW_ZATACKA;
325 }
326 if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru
327 {
328 STOPL;
329 GO(R, F, speed);
330 movement=L;
331 }
332 else
333 {
334 STOPR;
335 GO(L, F, speed);
336 movement=R;
337 }
338 } // while(true)
339 }
340