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