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 |
} |