Rev Author Line No. Line
503 kaklik 1 // Program pro predvadeni schopnosti robota Merkur
2 //------------------------------------------------
3  
4 #include "tank.h"
5  
6 unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
7 unsigned int8 line; // na ktere strane byla detekovana cara
8 unsigned int8 speed; // rychlost zataceni
9 unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo
10 unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku
11 int cirkus; // pocitadlo, po kolika akcich se ma delat cirkus
12 int1 BW; // urcuje, jestli je cara cerno/bila nebo
13 // bilo/cerna (true = bila cara, cerny podklad)
14  
15 // Konstanty pro dynamiku pohybu
16 #define T_DIRA 120 // po jakem case zataceni se detekuje dira
17 #define FW_POMALU 170 // trochu mimo caru vnitrni pas
18 #define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni
19 #define FW_STREDNE 190 // trochu mimo caru vnejsi pas
20 #define COUVANI 750 // couvnuti zpet na caru, po detekci diry
21 #define MAX_ROVINKA (255-FW_STREDNE)
22 #define TRESHOLD 15 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd
23 #define BUMPER_TRESHOLD 128 // rozhodovaci uroven cidla na prekazku
24  
25 //motory //Napred vypnout potom zapnout!
26 #define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred
27 #define FL output_low(PIN_B7); output_high(PIN_B6)
28 #define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad
29 #define BL output_low(PIN_B6); output_high(PIN_B7)
30 #define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav
31 #define STOPL output_low(PIN_B6);output_low(PIN_B7)
32  
33 #define L 0b10 // left
34 #define R 0b01 // right
35 #define S 0b11 // straight
36  
37 //cidla
38 #define RSENSOR (BW != C2OUT) // Senzory na caru
39 #define LSENSOR (BW != C1OUT)
40 #define BUMPER sAN2 // Senzor na cihlu
41  
42 #define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod
43 #define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod
44 #define BARVY PIN_B1 // Propojka pro nastaveni barvy cary
45  
46 #define SPEAKER PIN_B0 // vystup pro pipak
47  
48 #define LED1 PIN_A4 // LEDky
49 #define LED2 PIN_A3
50 #define LED3 PIN_A7
51 #define LED4 PIN_A6
52  
53 // makro pro PWM
54 #define GO(motor, direction, power) if(get_timer0()<=power) \
55 {direction##motor;} else {stop##motor;}
56  
57 #int_TIMER2
58 void TIMER2_isr() // obsluha zrychlovani
59 {
60 if (speed<255) speed++;
61 if (rovinka<MAX_ROVINKA) rovinka++;
62 }
63  
64 // Primitivni Pipani
65 void beep(unsigned int16 period, unsigned int16 length)
66 {
67 unsigned int16 nn;
68  
69 for(nn=length; nn>0; nn--)
70 {
71 output_high(SPEAKER);
72 delay_us(period);
73 output_low(SPEAKER);
74 delay_us(period);
75 }
76 }
77 /******************************************************************************/
78 void diagnostika()
79 {
80 unsigned int16 n;
81  
82 while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku
83 {
84 for (n=500; n<800; n+=100)
85 {
86 beep(n,n); //beep UP
87 };
88 Delay_ms(1000);
89 //zastav vse
90 STOPL; STOPR;
91 //pravy pas
92 FR; Delay_ms(1000); STOPR; Delay_ms(1000);
93 BR; Delay_ms(1000); STOPR; Delay_ms(1000);
94 Beep(880,100); Delay_ms(1000);
95 //levy pas
96 FL; Delay_ms(1000); STOPL; Delay_ms(1000);
97 BL; Delay_ms(1000); STOPL; Delay_ms(1000);
98 Beep(880,100); Delay_ms(1000);
99 //oba pasy
100 FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
101 BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
102 };
103 while (input(DIAG_SENSORS)) // spusteni diagnostiky cidel
104 {
105 if (RSENSOR) beep(1000,200);
106 Delay_ms(200);
107 if (LSENSOR) beep(2000,300);
108 Delay_ms(200);
109 if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(3000,400);
110 Delay_ms(200);
111 };
112 }
113 ///////////////////////////////////////////////////////////////////////////////
114 void OtocSe() // otoci se zpet, kdyz je prekazka
115 {
116 unsigned int16 n;
117  
118 BL; BR; // cukni zpatky
119 Delay_ms(200);
120 STOPR;STOPL;
121 beep(800,400);
122 beep(2000,1000);
123 output_low(LED4);
124 beep(900,400);
125 output_low(LED1);
126  
127 BR; FL; Delay_ms(100); // otoc se 30° do prava
128 STOPL; STOPR;
129 beep(1000,1000);
130 output_low(LED3);
131  
132 BR; FL;
133 for(n=40000;n>0;n--) // toc se, dokud nenarazis na caru
134 {
135 line = RSENSOR; // cteni senzoru na caru
136 line |= LSENSOR << 1;
137 if (line!=0) break;
138 }
139 STOPR; STOPL;
140 output_high(LED1); output_high(LED3); output_high(LED4);
141  
142 line=L; // caru jsme prejeli, tak je vlevo
143 cirkus=0;
144 }
145  
146  
147 void main()
148 {
149 unsigned int16 n; // pro FOR
150 unsigned int16 i;
151  
152 STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi
153  
154 setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator
155  
156 port_b_pullups(TRUE); // pullups pro piano na diagnostiku
157 setup_spi(FALSE);
158 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM
159  
160 setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci
161 // preruseni kazdych 10ms
162 setup_adc_ports(BUMPER|VSS_VDD); // nastaveni A/D prevodniku pro naraznik
163 setup_adc(ADC_CLOCK_INTERNAL);
164 set_adc_channel(2);
165 setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik
166 setup_ccp1(CCP_COMPARE_RESET_TIMER);
167 CCP_1=(2^10)-1; // prevod kazdou 1ms
168  
169 output_low(LED1); output_low(LED2); output_low(LED3); output_low(LED4);
170  
171 setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary
172 setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd
173  
174 Beep(1000,200); //double beep
175 Delay_ms(50);
176 Beep(1000,200);
177 Delay_ms(1000); // 1s
178  
179 // povoleni rizeni rychlosti zataceni pres preruseni
180 enable_interrupts(INT_TIMER2);
181 enable_interrupts(GLOBAL);
182  
183 /*---------------------------------------------------------------------------*/
184 sensors=S;
185 line=S;
186 last=S;
187 cirkus=0;
188 // movement=S;
189 speed=FW_POMALU;
190  
191 BW=input(BARVY); // Jaka ma byt barva cary?
192 diagnostika(); // Zkus, jestli nekdo nechce, diagnostiku
193 Delay_ms(500);
194  
195 output_high(LED1); Beep(1000,200); Delay_ms(500);
196 output_high(LED2); Beep(1000,200); Delay_ms(500);
197 output_high(LED3); Beep(1000,200); Delay_ms(500);
198 output_high(LED4); Beep(1000,200); Delay_ms(500);
199  
200 while(true) // hlavni smycka (jizda podle cary)
201 {
202 sensors = RSENSOR; // cteni senzoru na caru
203 sensors |= LSENSOR << 1;
204  
205 if (read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) OtocSe();
206  
207 switch (sensors) // zatacej podle toho, kde vidis caru
208 {
209 case S: // rovne
210 GO(L, F, FW_STREDNE+rovinka); GO(R, F, FW_STREDNE+rovinka);
211 continue;
212 case L: // trochu vlevo
213 GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
214 line=L;
215 continue;
216 case R: // trochu vpravo
217 GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
218 line=R;
219 continue;
220 default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal
221 };
222 rovinka=0;
223  
224 if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu,
225 // tak zabrzdi
226 {
227 output_bit(LED1, !input(LED1));
228 last=line;
229 speed=FW_ZATACKA;
230 cirkus++;
231 if (cirkus>8)
232 {
233 STOPL; STOPR;
234 cirkus=0;
235 disable_interrupts(GLOBAL);
236 beep(1000,400);
237 for(n=3000; n>3950; n--) beep(n,10);
238 output_low(LED1);
239 beep(2000,200);
240 beep(900,400);
241 for(n=2950; n<3000; n++) beep(n,10);
242 output_low(LED2);
243 output_high(LED1);
244 beep(4000,400);
245 beep(1000,100);
246 output_low(LED3);
247 beep(3000,400);
248 Delay_ms(1000);
249 output_high(LED1); output_high(LED2);
250 output_high(LED3); output_high(LED4);
251 enable_interrupts(GLOBAL);
252 }
253 };
254  
255 if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru
256 {
257 STOPL;
258 GO(R, F, speed);
259 }
260 else
261 {
262 STOPR;
263 GO(L, F, speed);
264 }
265  
266 } // while(true)
267 }
268