Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1 
#include "main.h"
2 
 
3 
#define  TXo PIN_C3                 // To the transmitter modulator
4 
#include "AX25.c"             // podprogram pro prenos telemetrie
5 
 
6 
//motory            //Napred vypnout potom zapnout!
7 
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
8 
#define FL         output_low(PIN_B7); output_high(PIN_B6)
9 
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
10 
#define BL         output_low(PIN_B6); output_high(PIN_B7)
11 
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)
12 
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
13 
 
14 
#define 	L 0b10  // left
15 
#define 	R 0b01  // right
16 
#define 	S 0b11  // straight
17 
 
18 
#define 	COUVANI		1600					// couvnuti po zjisteni diry
19 
#define 	MEZERA		5400					// za jak dlouho bude ztracena cara
20 
#define 	PRES_DIRU	400					// velikost mezery v care
21 
#define 	ODEZVA		1						// za jak dlouho po opusteni cary se ma zacit zatacet
22 
#define	BRZDENI		90						// doba (v ms) ptrebna k zastaveni jednoho motoru
23 
 
24 
//cidla
25 
#define 	RSENSOR    1       // Senzory na caru
26 
#define 	LSENSOR    0
27 
#define	BUMPER	PIN_C4		// sensor na cihlu
28 
 
29 
#define DIAG_SERVO      PIN_B0   // Propojka pro diagnosticky mod
30 
#define DIAG_SENSORS    PIN_B1   // Propojka pro diagnosticky mod
31 
 
32 
#DEFINE SOUND_HI   PIN_B3
33 
#DEFINE SOUND_LO   PIN_B2
34 
 
35 
char AXstring[40];   // Buffer pro prenos telemetrie
36 
 
37 
int tresholdL;		// rozhodovaci uroven pro prave cidlo
38 
int tresholdR;		// rozhodovaci uroven pro prave cidlo
39 
int movement;     // smer minuleho pohybu
40 
int line;         // na ktere strane byla detekovana cara
41 
unsigned int16 dira;			// pocitadlo pro nalezeni preruseni cary
42 
 
43 
// Primitivni Pipani
44 
void beep(unsigned int16 period, unsigned int16 length)
45 
{
46 
   unsigned int16 nn;
47 
 
48 
   for(nn=length; nn>0; nn--)
49 
   {
50 
     output_high(SOUND_HI);output_low(SOUND_LO);
51 
     delay_us(period);
52 
     output_high(SOUND_LO);output_low(SOUND_HI);
53 
     delay_us(period);
54 
   }
55 
}
56 
 
57 
// Diagnostika pohonu, hejbne vsema motorama ve vsech smerech
58 
void diagnostika()
59 
{
60 
   unsigned int16 n;
61 
 
62 
   while (input(DIAG_SERVO))   // Propojka, ktera spousti diagnostiku
63 
   {
64 
      for (n=500; n<800; n+=100)
65 
      {
66 
         beep(n,n); //beep UP
67 
      };
68 
      Delay_ms(1000);
69 
      //zastav vse
70 
      STOPL; STOPR;
71 
      //pravy pas
72 
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
73 
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
74 
      Beep(880,100); Delay_ms(1000);
75 
      //levy pas
76 
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
77 
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
78 
      Beep(880,100); Delay_ms(1000);
79 
      //oba pasy
80 
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
81 
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
82 
   };
83 
 
84 
   while (input(DIAG_SENSORS))
85 
   {
86 
      int ls, rs;
87 
		while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}
88 
      set_adc_channel(RSENSOR);
89 
      Delay_us(20);
90 
      rs=read_adc();
91 
      set_adc_channel(LSENSOR);
92 
      Delay_us(20);
93 
      ls=read_adc();
94 
      sprintf(AXstring,"L: %U  R: %U\0", ls, rs);  // Convert DATA to String.
95 
      SendPacket(&AXstring[0]);
96 
      delay_ms(1000);
97 
   };
98 
}
99 
 
100 
void cikcak()
101 
{
102 
int n;
103 
	switch(movement)												// podivej se na jednu stranu
104 
	{
105 
	case L:
106 
				FL;BR;
107 
				movement=R;
108 
			break;
109 
	case R:
110 
				FR;BL;
111 
				movement=L;
112 
			break;
113 
	case S:
114 
				FR;BL;
115 
				movement=L;
116 
			break;
117 
	}
118 
	set_adc_channel(LSENSOR);
119 
	Delay_us(10);
120 
	while (tresholdL < read_adc())						// je tam cara?
121 
	{
122 
		if (n==50)												// asi bude na druhe strane
123 
		{
124 
			STOPR;STOPL;
125 
			n=0;
126 
			switch(movement)
127 
			{
128 
			case L:
129 
						FL;BR;
130 
						movement=R;
131 
					break;
132 
			case R:
133 
						FR;BL;
134 
						movement=L;
135 
					break;
136 
			}
137 
		}
138 
		Delay_ms(5);
139 
		n++;
140 
	}
141 
	STOPL;STOPR;												// nasli jsme caru
142 
	line=S;
143 
}
144 
void objizdka()
145 
{
146 
	BL;BR;Delay_ms(300);
147 
	STOPR;STOPL;
148 
	beep(1000,1000);
149 
	Delay_ms(500);
150 
	beep(1000,1000);
151 
	Delay_ms(1000);
152 
 
153 
}
154 
void kalibrace()
155 
{
156 
unsigned int16 i;
157 
int min;
158 
int max;
159 
int current;
160 
int treshold;
161 
 
162 
	FL; BR; Delay_ms(130);
163 
chyba1:
164 
	FR; BL;													//kalibrace leveho cidla
165 
   set_adc_channel(LSENSOR);
166 
	Delay_us(20);
167 
	min=max=read_adc();
168 
	for (i=1;i<=500;i++)
169 
	{
170 
		current=read_adc();
171 
		if (max < current) max=current;
172 
		if (min > current) min=current;
173 
		Delay_us(500);
174 
	}
175 
	FL; BR;
176 
	for (i=1;i<=500;i++)
177 
	{
178 
		current=read_adc();
179 
		if (max < current) max=current;
180 
		if (min > current) min=current;
181 
		Delay_us(500);
182 
	}
183 
	STOPL; STOPR; Delay_ms(200);
184 
	if((max-min)<50) {Beep(1000,300); GOTO chyba1;}
185 
	treshold=(max-min)>>1;
186 
	tresholdL=treshold+min;
187 
 
188 
chyba2:
189 
	FR; BL;
190 
   set_adc_channel(RSENSOR);
191 
	Delay_us(20);
192 
	min=max=read_adc();									//naplneni min a max nejakou rozumnou hodnotou
193 
	for (i=1;i<=500 ;i++)
194 
	{
195 
		current=read_adc();
196 
		if (max < current) max=current;				//zmereni minima a maxima
197 
		if (min > current) min=current;
198 
		Delay_us(500);
199 
	}
200 
	FL; BR;
201 
	for (i=1;i<=500 ;i++)
202 
	{
203 
		current=read_adc();
204 
		if (max < current) max=current;				//zmereni minima a maxima
205 
		if (min > current) min=current;
206 
		Delay_us(500);
207 
	}
208 
	STOPL; STOPR; Delay_ms(200);
209 
	if((max-min)<50) {Beep(1000,300); GOTO chyba2;}
210 
	treshold=(max-min)>>1;
211 
	tresholdR=treshold+min;
212 
 
213 
	FR; BL;
214 
	movement=L;
215 
	set_adc_channel(LSENSOR);
216 
	Delay_us(20);
217 
	while (tresholdL < read_adc()) Delay_us(100);
218 
	FL; BR; Delay_ms(50);
219 
	STOPL; STOPR; Delay_ms(500);
220 
	Beep(780,200);
221 
}
222 
 
223 
void main()
224 
{
225 
unsigned int16 rovne;												// pocita delku rovne cary
226 
 
227 
   STOPL; STOPR;
228 
 
229 
   setup_adc_ports(RA0_RA1_RA3_ANALOG);
230 
   setup_adc(ADC_CLOCK_DIV_2);
231 
 
232 
   port_b_pullups(false);
233 
 
234 
	diagnostika();
235 
 
236 
   Beep(1000,200);     //double beep
237 
   Delay_ms(50);
238 
   Beep(1000,200);
239 
   Delay_ms(1000); // 1s
240 
 
241 
//	kalibrace();
242 
	tresholdl=tresholdr=80;
243 
//	FL; FR;
244 
   movement=S;
245 
   line=S;
246 
   dira=0;
247 
	rovne=0;
248 
 
249 
   while(true)
250 
   {
251 
		if(!input(BUMPER)) objizdka();
252 
		line=0;
253 
      set_adc_channel(RSENSOR);								// podivej se jestli neni cara pod pravym cidlem
254 
      Delay_us(10);
255 
      if(tresholdR > read_adc())
256 
		{
257 
			dira=0;
258 
			line=R;
259 
		}
260 
      set_adc_channel(LSENSOR);								// kdyz cara nebyla pod pravym cidlem, mozna bude pod levym
261 
      Delay_us(10);
262 
      if(tresholdL > read_adc())
263 
		{
264 
			dira=0;
265 
			line=line | L;
266 
		}
267 
 
268 
		switch(line)
269 
		{
270 
		case S:
271 
			FR;FL;
272 
			movement=S;
273 
			continue;
274 
		case L:
275 
			STOPL;
276 
			FR;movement=L;
277 
			continue;
278 
		case R:
279 
			STOPR;
280 
			FL;movement=R;
281 
			continue;
282 
		default:
283 
		}
284 
 
285 
		if (dira==ODEZVA)									// kdyz uz chvili jedeme po bile plose
286 
		{
287 
			//BR;BL;Delay_us(rovne >>= 5);
288 
			rovne=0;											//kdyz sme museli zatocit, uz neni rovna cara
289 
 
290 
			switch (line)									// musime zatocit
291 
			{
292 
		  	case L:
293 
        				BL;Delay_ms(BRZDENI);STOPL;
294 
						FR;
295 
						movement=L;
296 
					break;
297 
        	case R:
298 
        				BR;Delay_ms(BRZDENI);STOPR;
299 
						FL;
300 
						movement=R;
301 
					break;
302 
			}
303 
		}
304 
		if (dira==MEZERA)											// kdyz zkoncila cara
305 
		{
306 
			beep(800,500);
307 
			Delay_ms(50);
308 
			beep(800,500);
309 
			switch (movement)										//vrat se zpet na caru
310 
			{
311 
		  	case L:
312 
						STOPL;STOPR;
313 
        				BR;Delay_ms(COUVANI);STOPR;
314 
					break;
315 
        	case R:
316 
						STOPL;STOPR;
317 
        				BL;Delay_ms(COUVANI);STOPL;
318 
					break;
319 
			case S:
320 
						BL; BR; Delay_ms(COUVANI);
321 
						STOPL; STOPR;
322 
					break;
323 
			}
324 
 
325 
			FR;FL; Delay_ms(PRES_DIRU);						// popojedem dopredu mozna tam bude cara
326 
			STOPL; STOPR; movement=S;
327 
			cikcak();												// najdi caru
328 
			dira=0;
329 
		}
330 
		dira++;
331 
   } // while(true)
332 
}