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       100      // po jakem case zataceni se detekuje dira
18
#define INC_SPEED    1        // prirustek rychlosti v jednom kroku
19
#define FW_POMALU    100      // trochu mimo caru vnitrni pas
20
#define FW_ZATACKA   200      // rychlost vnejsiho kola pri zataceni
21
#define FW_STREDNE   110      // trochu mimo caru vnejsi pas
22
#define COUVANI      600      // 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
 
27
//motory            //Napred vypnout potom zapnout!
28
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
29
#define FL         output_low(PIN_B7); output_high(PIN_B6)
30
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
31
#define BL         output_low(PIN_B6); output_high(PIN_B7)
32
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav
33
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
34
 
35
#define L 0b10  // left
36
#define R 0b01  // right
37
#define S 0b11  // straight
38
 
39
//cidla
40
#define RSENSOR    C2OUT      // Senzory na caru
41
#define LSENSOR    C1OUT
42
#define BUMPER     PIN_A4     // Senzor na cihlu
43
 
44
#define DIAG_SERVO      PIN_B3   // Propojka pro diagnosticky mod
45
#define DIAG_SENSORS    PIN_B2   // Propojka pro diagnosticky mod
46
 
47
#DEFINE SOUND_HI   PIN_A6     // komplementarni vystupy pro piezo pipak
48
#DEFINE SOUND_LO   PIN_A7
49
 
50
char AXstring[40];   // Buffer pro prenos telemetrie
51
 
52
// makro pro PWM
53
#define GO(motor, direction, power) if(get_timer0()<=power) \
54
                                          {direction##motor;} \
55
                                          else \
56
                                          {stop##motor;}
57
 
58
#int_TIMER2
59
void TIMER2_isr()
60
{
61
   if (speed<255) speed+=INC_SPEED;
62
   if (rovinka<MAX_ROVINKA) rovinka++;
63
   if (dira<T_DIRA) dira++;
64
}
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
 
106
   while (input(DIAG_SENSORS))
107
   {
108
      int ls, rs;
109
		while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}
110
      set_adc_channel(RSENSOR);
111
      Delay_us(20);
112
      rs=read_adc();
113
      set_adc_channel(LSENSOR);
114
      Delay_us(20);
115
      ls=read_adc();
116
      sprintf(AXstring,"L: %U  R: %U\0", ls, rs);  // Convert DATA to String.
117
      SendPacket(&AXstring[0]);
118
      delay_ms(1000);
119
   };
120
}
121
///////////////////////////////////////////////////////////////////////////////
122
void cikcak()
123
{
124
int n;
125
	switch(movement)									// podivej se na jednu stranu
126
	{
127
	case L:
128
				FL;BR;
129
				movement=R;
130
			break;
131
	case R:
132
				FR;BL;
133
				movement=L;
134
			break;
135
	case S:
136
				FR;BL;
137
				movement=L;
138
			break;
139
	}
140
	while (0==(RSENSOR|LSENSOR))
141
	{
142
		if (n==50)							//cara asi bude na druhe strane
143
		{
144
			STOPR;STOPL;
145
			n=0;
146
			switch(movement)
147
			{
148
			case L:
149
						FL;BR;
150
						movement=R;
151
					break;
152
			case R:
153
						FR;BL;
154
						movement=L;
155
					break;
156
			}
157
		}
158
		Delay_ms(5);
159
		n++;
160
	}
161
	STOPL;STOPR;												// nasli jsme caru
162
	line=S;
163
}
164
///////////////////////////////////////////////////////////////////////////////
165
void objizdka()
166
{
167
	BL;BR;Delay_ms(300);
168
	STOPR;STOPL;
169
	beep(1000,1000);
170
	Delay_ms(500);
171
	beep(1000,1000);
172
	Delay_ms(1000);
173
 
174
 
175
 
176
}
177
///////////////////////////////////////////////////////////////////////////////
178
void prejeddiru()
179
{
180
   STOPL;STOPR;
181
   beep(800,500);
182
   Delay_ms(50);
183
   beep(800,500);
184
   switch (movement)                            //vrat se zpet na caru
185
   {
186
   case L:
187
         BR;Delay_ms(COUVANI);STOPR;
188
         STOPL;STOPR;
189
      break;
190
   case R:
191
 
192
         BL;Delay_ms(COUVANI);STOPL;
193
         STOPL;STOPR;
194
      break;
195
   case S:
196
         BL; BR; Delay_ms(COUVANI);
197
         STOPL; STOPR;
198
      break;
199
   }
200
 
201
   FR;FL; Delay_ms(PRES_DIRU);   // popojedem dopredu mozna tam bude cara
202
   STOPL; STOPR;
203
   cikcak();                                    // najdi caru
204
   dira=0;
205
}
206
///////////////////////////////////////////////////////////////////////////////
207
void main()
208
{
209
   unsigned int16 n; // pro FOR
210
 
211
   STOPL; STOPR;     // prepne vystupy na ovladani motoru na output a zastavi
212
 
213
   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator
214
 
215
   port_b_pullups(TRUE);      // pullups pro piano na diagnostiku
216
   setup_spi(FALSE);
217
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM
218
 
219
   setup_timer_2(T2_DIV_BY_16,255,10);    // Casovac pro regulaci
220
                                         // preruseni kazdych 10ms
221
   setup_adc_ports(sAN2|VSS_VDD);      // nastaveni A/D prevodniku pro naraznik
222
   setup_adc(ADC_CLOCK_INTERNAL);
223
   set_adc_channel(2);
224
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro naraznik
225
   setup_ccp1(CCP_COMPARE_RESET_TIMER);
226
   CCP_1=(2^10)-1;                        // prevod kazdou 1ms
227
 
228
   setup_comparator(A0_VR_A1_VR);   // inicializace komparatoru pro cidla cary
229
   setup_vref(VREF_HIGH|TRESHOLD);        // 32 kroku od 0.25 do 0.75 Vdd
230
 
231
   Beep(1000,200);     //double beep
232
   Delay_ms(50);
233
   Beep(1000,200);
234
   Delay_ms(1000);      // 1s
235
 
236
         // povoleni rizeni rychlosti zataceni pres preruseni
237
   enable_interrupts(INT_TIMER2);
238
   enable_interrupts(GLOBAL);
239
 
240
/*---------------------------------------------------------------------------*/
241
   sensors=S;
242
   line=S;
243
   last=S;
244
   movement=S;
245
   speed=FW_POMALU;
246
 
247
   diagnostika();
248
   cikcak();     // toc se, abys nasel caru
249
   Delay_ms(500);
250
   Beep(1000,200);
251
   Delay_ms(500);
252
 
253
   while(true)       // hlavni smycka (jizda podle cary)
254
   {
255
 
256
      sensors = RSENSOR;         // cteni senzoru na caru
257
      sensors |= LSENSOR << 1;
258
 
259
      switch (sensors)  // zatacej podle toho, kde vidis caru
260
      {
261
         case S:                          // rovne
262
            GO(L, F, speed); GO(R, F, speed);
263
//            FL; FR;  // pokud se jede dlouho rovne, tak pridej
264
            dira=0;
265
            continue;
266
 
267
         case L:                          // trochu vlevo
268
            GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
269
            line=L;
270
            dira=0;
271
            continue;
272
 
273
         case R:                          // trochu vpravo
274
            GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
275
            line=R;
276
            dira=0;
277
            continue;
278
 
279
         default:       // kdyz jsou obe cidla mimo caru, tak pokracuj dal
280
      }
281
   rovinka=0;
282
//      if (dira>=T_DIRA) prejeddiru();
283
      if (last!=line)     // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
284
      {
285
         last=line;
286
         speed=FW_ZATACKA;
287
      }
288
      else speed=255;
289
 
290
/*      if (dira==0)
291
      {
292
         if (L==line)  // kdyz jsou obe cidla mimo caru, zabrzdi vnitrni kolo
293
         {
294
            BL;
295
            for(n=4000;n>0;n--)           // Delay
296
            {
297
               line = RSENSOR;         // precteni cidel
298
               line |= LSENSOR << 1;      // sestav informaci o care
299
               if(line!=0) break;
300
            }
301
            STOPL;
302
         }
303
         else
304
         {
305
            BR;
306
            for(n=4000;n>0;n--)           // Delay
307
            {
308
               line = RSENSOR;         // precteni cidel
309
               line |= LSENSOR << 1;      // sestav informaci o care
310
               if(line!=0) break;
311
            }
312
            STOPR;
313
        }
314
      }*/
315
 
316
      if (L==line)  // kdyz jsou obe cidla mimo caru, zatoc na caru
317
      {
318
         STOPL;
319
         GO(R, F, speed);
320
         movement=L;
321
      }
322
      else
323
      {
324
         STOPR;
325
         GO(L, F, speed);
326
         movement=R;
327
 
328
      }
329
   } // while(true)
330
}
331