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
}