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       120      // 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   240      // rychlost vnejsiho kola pri zataceni
21
#define FW_STREDNE   240      // trochu mimo caru vnejsi pas
22
#define COUVANI      750      // couvnuti zpet na caru, po detekci diry
23
#define PRES_DIRU    300
24
#define MAX_ROVINKA  (255-FW_STREDNE)
25
#define TRESHOLD     6        // 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))         // spusteni diagnostiky cidel
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(150);
155
	STOPR;STOPL;
156
	beep(900,1000);
157
  // movement=S;
158
  // cikcak();
159
 
160
   BR; FL; Delay_ms(270);           // otoc se 70° do prava
161
 
162
   FR; FL; Delay_ms(500);           // popojed rovne
163
 
164
   BL; Delay_ms(30);               // otoc se 90° do leva
165
   STOPL; FR; Delay_ms(500);
166
 
167
   FR; FL;  Delay_ms(100);           // popojed rovne na slepo
168
   for(n=40000;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(150); break;}
173
//     Delay_ms(1);
174
   }
175
 
176
   BR; FL;                             // otoc se 60° do prava
177
   for(n=40000;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=L; //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(1000,500);
214
 
215
   /*line=0;
216
   FR; BL; Delay_ms(400);                 // otoc se na caru
217
   beep(1000,500);
218
   while(line==0)
219
   {
220
      line = RSENSOR;         // cteni senzoru na caru
221
      line |= LSENSOR << 1;
222
   }
223
   FL;BR; Delay_ms(60);       // zabrzdi
224
   STOPL; STOPR;
225
 
226
   FL; BR; Delay_ms(700);     // otacka 180 deg
227
   STOPL; STOPR;*/
228
 
229
   FR;FL;                     //popojed rovne
230
   for(n=PRES_DIRU;n>0;n--)
231
   {
232
      line = RSENSOR;         // cteni senzoru na caru
233
      line |= LSENSOR << 1;
234
      if (line!=0) break;
235
      Delay_ms(1);
236
   }
237
sem:
238
   STOPL; STOPR;
239
   movement=S;
240
   cikcak();                                    // najdi caru
241
   dira=0;
242
}
243
///////////////////////////////////////////////////////////////////////////////
244
void main()
245
{
246
   unsigned int16 n; // pro FOR
247
 
248
   STOPL; STOPR;     // prepne vystupy na ovladani motoru na output a zastavi
249
 
250
   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator
251
 
252
   port_b_pullups(TRUE);      // pullups pro piano na diagnostiku
253
   setup_spi(FALSE);
254
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM
255
 
256
   setup_timer_2(T2_DIV_BY_4,255,10);    // Casovac pro regulaci
257
                                         // preruseni kazdych 10ms
258
   setup_adc_ports(sAN2|VSS_VDD);      // nastaveni A/D prevodniku pro naraznik
259
   setup_adc(ADC_CLOCK_INTERNAL);
260
   set_adc_channel(2);
261
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro naraznik
262
   setup_ccp1(CCP_COMPARE_RESET_TIMER);
263
   CCP_1=(2^10)-1;                        // prevod kazdou 1ms
264
 
265
   setup_comparator(A0_VR_A1_VR);   // inicializace komparatoru pro cidla cary
266
   setup_vref(VREF_HIGH|TRESHOLD);        // 32 kroku od 0.25 do 0.75 Vdd
267
 
268
   Beep(1000,200);     //double beep
269
   Delay_ms(50);
270
   Beep(1000,200);
271
   Delay_ms(1000);      // 1s
272
 
273
         // povoleni rizeni rychlosti zataceni pres preruseni
274
   enable_interrupts(INT_TIMER2);
275
   enable_interrupts(GLOBAL);
276
 
277
/*---------------------------------------------------------------------------*/
278
   sensors=S;
279
   line=S;
280
   last=S;
281
   movement=S;
282
   speed=FW_POMALU;
283
 
284
   diagnostika();
285
   //cikcak();     // toc se, abys nasel caru
286
   Delay_ms(500);
287
   Beep(1000,200);
288
   Delay_ms(500);
289
 
290
   while(true)       // hlavni smycka (jizda podle cary)
291
   {
292
      sensors = RSENSOR;         // cteni senzoru na caru
293
      sensors |= LSENSOR << 1;
294
 
295
      if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && (dira<=T_CIHLA)) objizdka();
296
 
297
      switch (sensors)  // zatacej podle toho, kde vidis caru
298
      {
299
         case S:                          // rovne
300
            FL; FR;  // pokud se jede dlouho rovne, tak pridej
301
            dira=0;
302
            movement=S;
303
            continue;
304
         case L:                          // trochu vlevo
305
            GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
306
            line=L;
307
            dira=0;
308
            movement=L;
309
            continue;
310
         case R:                          // trochu vpravo
311
            GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
312
            line=R;
313
            dira=0;
314
            movement=R;
315
            continue;
316
         default:       // kdyz jsou obe cidla mimo caru, tak pokracuj dal
317
      }
318
   rovinka=0;
319
      if (dira>=T_DIRA) prejeddiru();
320
      if (last!=line)     // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
321
      {
322
         last=line;
323
         speed=FW_ZATACKA;
324
      }
325
      if (L==line)  // kdyz jsou obe cidla mimo caru, zatoc na caru
326
      {
327
         STOPL;
328
         GO(R, F, speed);
329
         movement=L;
330
      }
331
      else
332
      {
333
         STOPR;
334
         GO(L, F, speed);
335
         movement=R;
336
      }
337
   } // while(true)
338
}
339