Subversion Repositories svnkaklik

Rev

Go to most recent revision | 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