Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 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(300);
107
      if (LSENSOR) beep(2000,300);
108
      Delay_ms(300);
109
      if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(3000,400);
110
      Delay_ms(300);
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