Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1
////////////////////////////////////////////////////////////////////////////////
2
//                                                                            //
3
//			             Program pro robota Merkur                               //
4
//                                                                            //
5
////////////////////////////////////////////////////////////////////////////////
6
#include "merkur.h"
7
 
8
//#define DEBUG
9
 
10
#define  TXo PIN_A3           // To the transmitter modulator
11
#include "AX25.c"             // podprogram pro prenos telemetrie
12
 
13
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
14
unsigned int8 line;           // na ktere strane byla detekovana cara
15
unsigned int8 speed;          // rychlost zataceni
16
unsigned int8 rovinka;        // pocitadlo pro zjisteni rovneho useku
17
unsigned int8 last;           // kde byla cara, kdyz byly minule cidla mimo
18
unsigned int8 movement;       // obsahuje aktualni smer zataceni
19
unsigned int8 dira;           // pocita dobu po kterou je ztracena cara
20
 
21
// Konstanty pro dynamiku pohybu
22
#define T_DIRA       120      // po jakem case zataceni se detekuje dira
23
#define INC_SPEED    1        // prirustek rychlosti v jednom kroku
24
#define FW_POMALU    230      // trochu mimo caru vnitrni pas
25
#define FW_ZATACKA   240      // rychlost vnejsiho kola pri zataceni
26
#define FW_STREDNE   240      // trochu mimo caru vnejsi pas
27
#define COUVANI      750      // couvnuti zpet na caru, po detekci diry
28
#define PRES_DIRU    300      // predpokladana velikost diry
29
#define MAX_ROVINKA  (255-FW_STREDNE)     // maximalni rychlost na rovince
30
#define TRESHOLD     6        // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd
31
#define BUMPER_TRESHOLD 128   // rozhodovaci uroven cidla na cihlu
32
#define CIK_CAK      30000    // maximalni rozkmit pri hledani cary
33
#define T_CIHLA      50       // perioda detekce cihly
34
 
35
//motory            //Napred vypnout potom zapnout!
36
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
37
#define FL         output_low(PIN_B7); output_high(PIN_B6)
38
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
39
#define BL         output_low(PIN_B6); output_high(PIN_B7)
40
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav
41
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
42
 
43
#define L 0b10  // left
44
#define R 0b01  // right
45
#define S 0b11  // straight
46
 
47
//cidla
48
#define RSENSOR    C2OUT      // Senzory na caru
49
#define LSENSOR    C1OUT
50
#define BUMPER     PIN_A4     // Senzor na cihlu
51
 
52
#define DIAG_SERVO      PIN_B3   // Propojka pro diagnosticky mod
53
#define DIAG_SENSORS    PIN_B2   // Propojka pro diagnosticky mod
54
 
55
#DEFINE SOUND_HI   PIN_A6     // komplementarni vystupy pro piezo pipak
56
#DEFINE SOUND_LO   PIN_A7
57
 
58
char AXstring[40];   // Buffer pro prenos telemetrie
59
 
60
// makro pro PWM
61
#define GO(motor, direction, power) if(get_timer0()<=power) \
62
{direction##motor;} else {stop##motor;}
63
 
64
#int_TIMER2
65
void TIMER2_isr()
66
{
67
   if (speed<255) speed+=INC_SPEED;
68
   if (rovinka<MAX_ROVINKA) rovinka++;
69
   if (dira<=T_DIRA) dira++;
70
}
71
// Primitivni Pipani
72
void beep(unsigned int16 period, unsigned int16 length)
73
{
74
   unsigned int16 nn;
75
 
76
   for(nn=length; nn>0; nn--)
77
   {
78
     output_high(SOUND_HI);output_low(SOUND_LO);
79
     delay_us(period);
80
     output_high(SOUND_LO);output_low(SOUND_HI);
81
     delay_us(period);
82
   }
83
}
84
/******************************************************************************/
85
void diagnostika() // vyzkousi funkci pohonu a cidel
86
{
87
   unsigned int16 n;
88
 
89
   while (input(DIAG_SERVO))   // Propojka, ktera spousti diagnostiku
90
   {
91
      for (n=500; n<800; n+=100)
92
      {
93
         beep(n,n); //beep UP
94
      };
95
      Delay_ms(1000);
96
      //zastav vse
97
      STOPL; STOPR;
98
      //pravy pas
99
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
100
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
101
      Beep(880,100); Delay_ms(1000);
102
      //levy pas
103
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
104
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
105
      Beep(880,100); Delay_ms(1000);
106
      //oba pasy
107
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
108
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
109
   };
110
   while (input(DIAG_SENSORS))         // spusteni diagnostiky cidel
111
   {
112
      if (RSENSOR) beep(900,500);
113
      if (LSENSOR) beep(800,500);
114
      if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(1000,500);
115
   };
116
}
117
///////////////////////////////////////////////////////////////////////////////
118
void cikcak()                 // najde ztracenou caru
119
{
120
unsigned int16 n;
121
sem1:
122
   n=CIK_CAK;
123
	while (0==RSENSOR||LSENSOR)       // zkontroluj caru
124
	{
125
		if (n==CIK_CAK)							// zmen smer zataceni
126
		{
127
			n=0;
128
			switch(movement)
129
			{
130
			case L:
131
						FL;BR;
132
						movement=R;
133
					break;
134
			case R:
135
						FR;BL;
136
						movement=L;
137
					break;
138
         case S:
139
				     FL;BR;
140
				     movement=R;
141
                 n=CIK_CAK/2;
142
			      break;
143
			}
144
		}
145
		n++;
146
	}
147
	STOPL;STOPR;
148
   line = RSENSOR;         // cteni senzoru na caru
149
   line |= LSENSOR << 1;
150
   if (line==0) goto sem1;
151
   												// nasli jsme caru
152
	line=S;
153
}
154
///////////////////////////////////////////////////////////////////////////////
155
void objizdka()                     // objede cihlu
156
{
157
unsigned int16 n;
158
 
159
	BL;BR;Delay_ms(150);
160
	STOPR;STOPL;
161
	beep(900,1000);
162
  // movement=S;
163
  // cikcak();
164
 
165
   BR; FL; Delay_ms(270);           // otoc se 70° do prava
166
 
167
   FR; FL; Delay_ms(500);           // popojed rovne
168
 
169
   BL; Delay_ms(30);               // otoc se 90° do leva
170
   STOPL; FR; Delay_ms(500);
171
 
172
   FR; FL;  Delay_ms(100);           // popojed rovne na slepo
173
   for(n=40000;n>0;n--)               // popojed rovne ale kontroluj caru
174
   {
175
      line = RSENSOR;         // cteni senzoru na caru
176
      line |= LSENSOR << 1;
177
      if (line!=0) {Delay_ms(150); break;}
178
//     Delay_ms(1);
179
   }
180
 
181
   BR; FL;                             // otoc se 60° do prava
182
   for(n=40000;n>0;n--)
183
   {
184
      line = RSENSOR;         // cteni senzoru na caru
185
      line |= LSENSOR << 1;
186
      if (line!=0) break;
187
//      Delay_ms(1);
188
   }
189
   STOPR; STOPL;
190
 
191
   movement=L; //R;
192
   cikcak();
193
   dira=0;
194
}
195
///////////////////////////////////////////////////////////////////////////////
196
void prejeddiru()                    // vyresi diru
197
{
198
unsigned int16 n;
199
unsigned int8 speed_dira;
200
 
201
   STOPL;STOPR;
202
   speed_dira=speed;
203
   beep(1000,500);
204
   switch (movement)                            //vrat se zpet na caru
205
   {
206
   case L:
207
         for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);}
208
         STOPL;STOPR;
209
      break;
210
   case R:
211
         for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);}
212
         STOPL;STOPR;
213
      break;
214
   case S:
215
         goto sem;
216
      break;
217
   }
218
   beep(1000,500);
219
 
220
   FR;FL;                     //popojed rovne
221
   for(n=PRES_DIRU;n>0;n--)
222
   {
223
      line = RSENSOR;         // cteni senzoru na caru
224
      line |= LSENSOR << 1;
225
      if (line!=0) break;
226
      Delay_ms(1);
227
   }
228
sem:
229
   STOPL; STOPR;
230
   movement=S;
231
   cikcak();           // najdi caru
232
   dira=0;
233
}
234
///////////////////////////////////////////////////////////////////////////////
235
void main()
236
{
237
   unsigned int16 n; // pro FOR
238
 
239
   STOPL; STOPR;     // prepne vystupy na ovladani motoru na output a zastavi
240
 
241
   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator
242
 
243
   port_b_pullups(TRUE);      // pullups pro piano na diagnostiku
244
   setup_spi(FALSE);
245
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM
246
 
247
   setup_timer_2(T2_DIV_BY_4,255,10);    // Casovac pro regulaci
248
                                         // preruseni kazdych 10ms
249
   setup_adc_ports(sAN2|VSS_VDD);      // nastaveni A/D prevodniku pro naraznik
250
   setup_adc(ADC_CLOCK_INTERNAL);
251
   set_adc_channel(2);
252
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro naraznik
253
   setup_ccp1(CCP_COMPARE_RESET_TIMER);
254
   CCP_1=(2^10)-1;                        // prevod kazdou 1ms
255
 
256
   setup_comparator(A0_VR_A1_VR);   // inicializace komparatoru pro cidla cary
257
   setup_vref(VREF_HIGH|TRESHOLD);        // 32 kroku od 0.25 do 0.75 Vdd
258
 
259
   Beep(1000,200);     //double beep
260
   Delay_ms(50);
261
   Beep(1000,200);
262
   Delay_ms(1000);      // 1s
263
 
264
         // povoleni rizeni rychlosti zataceni pres preruseni
265
   enable_interrupts(INT_TIMER2);
266
   enable_interrupts(GLOBAL);
267
 
268
/*---------------------------------------------------------------------------*/
269
   sensors=S;
270
   line=S;
271
   last=S;
272
   movement=S;
273
   speed=FW_POMALU;
274
 
275
   diagnostika();
276
   //cikcak();     // toc se, abys nasel caru
277
   Delay_ms(500);
278
   Beep(1000,200);
279
   Delay_ms(500);
280
 
281
   while(true)       // hlavni smycka (jizda podle cary)
282
   {
283
      sensors = RSENSOR;         // cteni senzoru na caru
284
      sensors |= LSENSOR << 1;
285
 
286
      if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && \
287
         (dira<=T_CIHLA)) objizdka();
288
 
289
      switch (sensors)  // zatacej podle toho, kde vidis caru
290
      {
291
         case S:                          // rovne
292
            FL; FR;  // pokud se jede dlouho rovne, tak pridej
293
            dira=0;
294
            movement=S;
295
            continue;
296
         case L:                          // trochu vlevo
297
            GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
298
            line=L;
299
            dira=0;
300
            movement=L;
301
            continue;
302
         case R:                          // trochu vpravo
303
            GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
304
            line=R;
305
            dira=0;
306
            movement=R;
307
            continue;
308
         default:       // kdyz jsou obe cidla mimo caru, tak pokracuj dal
309
      }
310
   rovinka=0;
311
      if (dira>=T_DIRA) prejeddiru();
312
      if (last!=line)     // pokud si prejel caru z jedne strany na druhou
313
                          // stranu, tak zabrzdi
314
      {
315
         last=line;
316
         speed=FW_ZATACKA;
317
      }
318
      if (L==line)  // kdyz jsou obe cidla mimo caru, zatoc na caru
319
      {
320
         STOPL;
321
         GO(R, F, speed);
322
         movement=L;
323
      }
324
      else
325
      {
326
         STOPR;
327
         GO(L, F, speed);
328
         movement=R;
329
      }
330
   } // while(true)
331
}
332