Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 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