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
}