Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1 
#include ".\main.h"
2 
 
3 
#use rtos (timer=2, minor_cycle=2ms)
4 
 
5 
#define  STRED       128      // sredni poloha zadniho kolecka
6 
#define  BEAR1       12       // 3 stupne zataceni
7 
#define  BEAR2       34
8 
#define  BEAR3       70
9 
#define  SPEEDMAX    120      // maximalni rychlost
10 
 
11 
#define  L           1
12 
#define  S           2
13 
#define  R           0
14 
 
15 
// servo
16 
#define  SERVO PIN_A2
17 
 
18 
// IR
19 
#define IRTX      PIN_B2
20 
 
21 
//motory
22 
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
23 
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
24 
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
25 
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
26 
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
27 
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)
28 
 
29 
//HID
30 
#define  LED1     PIN_B1      //oranzova
31 
#define  LED2     PIN_B2      //zluta
32 
 
33 
#define  STROBE   PIN_B0
34 
//#define  SW1      PIN_A2      // Motory On/off
35 
 
36 
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
37 
unsigned int8 line;           // na ktere strane byla detekovana cara
38 
//unsigned int8 dira;           // pocita dobu po kterou je ztracena cara
39 
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
40 
//unsigned int8 speed;          // maximalni povolena rychlost
41 
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky
42 
 
43 
signed int16  Lmotor;         // promene, ktere urcuji velikost vykonu na levem
44 
signed int16  Rmotor;         // a pravem motoru
45 
 
46 
// makro pro PWM
47 
#define GO(motor, direction, power) if(get_timer0()<=power) \
48 
{direction##motor;} else {stop##motor;}
49 
 
50 
////////////////////////////////////////////////////////////////////////////////
51 
////////////////////////////////////////////////////////////////////////////////
52 
/*
53 
void diagnostika()
54 
{
55 
   if(input(SW1))STOPR;STOPL;While(TRUE);
56 
//   if(LSENSOR==true) output_high(LED2); else output_low(LED2);
57 
//   if(RSENSOR==true) output_high(LED1); else output_low(LED1);
58 
}
59 
*/
60 
////////////////////////////////////////////////////////////////////////////////
61 
#task (rate=18ms,max=2ms)
62 
void zatoc()   // ridi servo ktere otaci kolem
63 
{
64 
   unsigned int8 n;
65 
 
66 
   output_high(SERVO);
67 
   delay_us(1100);
68 
   for(n=uhel; n>0; n--);
69 
   output_low(SERVO);
70 
}
71 
 
72 
////////////////////////////////////////////////////////////////////////////////
73 
void diag()   // ridi servo ktere otaci kolem
74 
{
75 
   unsigned int8 n;
76 
 
77 
   output_high(SERVO);
78 
   delay_us(1100);
79 
   for(n=uhel; n>0; n--);
80 
   output_low(SERVO);
81 
}
82 
 
83 
////////////////////////////////////////////////////////////////////////////////
84 
short int IRcheck()                 // potvrdi detekci cihly
85 
{
86 
   output_high(IRTX);               // vypne vysilac IR
87 
 
88 
   output_low(STROBE);
89 
   sensors = spi_read(0);         // cteni senzoru
90 
   output_high(STROBE);
91 
 
92 
   if(true==bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal
93 
   {
94 
      output_low(IRTX);             // zapne vysilac IR
95 
 
96 
      output_low(STROBE);
97 
      sensors = spi_read(0);         // cteni senzoru
98 
      output_high(STROBE);
99 
 
100 
      if(false==bit_test(sensors,7))      // otestuje, jestli je detekovana cihla
101 
      {
102 
         output_high(IRTX);            // vypne vysilac IR
103 
 
104 
         output_low(STROBE);
105 
         sensors = spi_read(0);         // cteni senzoru
106 
         output_high(STROBE);
107 
 
108 
         if(bit_test(sensors,7)) return 1; //
109 
 
110 
         else return 0; // vrat 0 kdyz je ruseni
111 
      }
112 
      else return 0; // vrat 0 kdyz to nebyla cihla
113 
   }
114 
   else return 0; // vrat 0 kdyz je detekovano ruseni
115 
}
116 
////////////////////////////////////////////////////////////////////////////////
117 
void ojizdka()
118 
{
119 
 
120 
}
121 
////////////////////////////////////////////////////////////////////////////////
122 
/*
123 
#task (rate=4ms, max=20us)
124 
void rychlost()
125 
{
126 
   if(speed<255) speed++;
127 
}
128 
*/
129 
////////////////////////////////////////////////////////////////////////////////
130 
#task (rate=2ms, max=1ms)
131 
void rizeni()
132 
{
133 
   unsigned int16 n;
134 
   unsigned int8 i;
135 
 
136 
   GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory
137 
 
138 
   delay_us(500);
139 
 
140 
   output_low(STROBE);
141 
   sensors = spi_read(0);         // cteni senzoru
142 
   sensors=~sensors;
143 
   output_high(STROBE);
144 
 
145 
   i=0;                    // havarijni kod
146 
   for (n=0; n<=6; n++)
147 
   {
148 
      if(bit_test(sensors,n)) i++;
149 
   }
150 
   if (i>3) rtos_terminate(); // zastavi, kdyz je cerno pod vice nez tremi cidly
151 
 
152 
 
153 
   if(bit_test(sensors,3)) //...|...//
154 
   {
155 
      uhel=STRED;
156 
      Lmotor=SPEEDMAX;
157 
      Rmotor=SPEEDMAX;
158 
      line=S;
159 
      if (rovinka<255) rovinka++;
160 
      return;
161 
   }
162 
 
163 
   if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
164 
   {
165 
      uhel=STRED-BEAR3;
166 
      Lmotor=0;
167 
      Rmotor=SPEEDMAX;
168 
      line=L;
169 
      return;
170 
   }
171 
 
172 
   if(bit_test(sensors,6)) //......|//
173 
   {
174 
      uhel=STRED+BEAR3;
175 
      Rmotor=0;
176 
      Lmotor=SPEEDMAX;
177 
      line=R;
178 
      return;
179 
 
180 
   }
181 
 
182 
   if(bit_test(sensors,1)) //.|.....//
183 
   {
184 
      uhel=STRED-BEAR2;
185 
      Lmotor=SPEEDMAX-50;
186 
      Rmotor=SPEEDMAX;
187 
      line=S;
188 
      return;
189 
   }
190 
 
191 
   if(bit_test(sensors,5)) //.....|.//
192 
   {
193 
      uhel=STRED+BEAR2;
194 
      Rmotor=SPEEDMAX-50;
195 
      Lmotor=SPEEDMAX;
196 
      line=S;
197 
      return;
198 
   }
199 
 
200 
   if (bit_test(sensors,2)) //..|....//
201 
   {
202 
      uhel=STRED-BEAR1;
203 
      Lmotor=SPEEDMAX;
204 
      Rmotor=SPEEDMAX;
205 
      line=S;
206 
      if (rovinka<255) rovinka++;
207 
      return;
208 
   }
209 
 
210 
   if (bit_test(sensors,4)) //....|..//
211 
   {
212 
      uhel=STRED+BEAR1;
213 
      Rmotor=SPEEDMAX;
214 
      Lmotor=SPEEDMAX;
215 
      line=S;
216 
      if (rovinka<255) rovinka++;
217 
      return;
218 
   }
219 
 
220 
   if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
221 
   {
222 
      if (rovinka>250)
223 
      {
224 
         BL; BR;
225 
         for(n=1; n<(60); n++) {rtos_yield(); delay_us(500);};
226 
      };
227 
      rovinka=0;
228 
   }
229 
 
230 
   if(bit_test(sensors,7))       // zjisti jestli neni cihla
231 
   {
232 
      rtos_terminate();
233 
   }
234 
}
235 
 
236 
 
237 
 
238 
////////////////////////////////////////////////////////////////////////////////
239 
void main()
240 
{
241 
   unsigned int16 i;
242 
   unsigned int8 last;
243 
 
244 
   setup_adc_ports(NO_ANALOGS);
245 
   setup_adc(ADC_CLOCK_INTERNAL);
246 
    setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
247 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
248 
   setup_timer_1(T1_DISABLED);
249 
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
250 
 
251 
   STOPR; STOPL;  // zastav motory
252 
   Lmotor=0;Rmotor=0;
253 
 
254 
   uhel=STRED;    // nastav zadni kolecko na stred
255 
   rovinka=0;
256 
 
257 
   output_low(IRTX); // zapni IR vysilac
258 
 
259 
   for(i=0;i<100;i++)   // pockej, nez se zadni kolecko vystredi
260 
   {
261 
      diag();
262 
      delay_ms(16);
263 
   }
264 
//      diagnostika();
265 
 
266 
   while(true)
267 
   {
268 
      rtos_run();
269 
      STOPR;
270 
      STOPL;
271 
      If(IRcheck()) objizdka();
272 
 
273 
      while(true);
274 
   }
275 
}