Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1
#include ".\main.h"
2
 
3
#define  KOLMO1      225         // predni kolecko sroubem dopredu
4
#define  KOLMO2      30          // predni kolecko je hlavou sroubu dozadu
5
#define  STRED       128      // sredni poloha zataceciho kolecka
6
#define  BEAR1       12       // 3 stupne zataceni
7
#define  BEAR2       34
8
#define  BEAR3       55
9
#define  SPEEDMAX    140      // maximalni rychlost
10
 
11
#define  L           1     // cara vlevo
12
#define  S           2     // casa mezi sensory
13
#define  R           0     // cara vpravo
14
 
15
// servo
16
#define  SERVO PIN_A2
17
 
18
// IR
19
#define IRTX      PIN_B2
20
#define  CIHLA    PIN_A3
21
 
22
//motory
23
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
24
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
25
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
26
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
27
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
28
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)
29
 
30
//HID
31
#define  LED1     PIN_B1      //oranzova
32
#define  LED2     PIN_B2      //zluta
33
 
34
#define  STROBE   PIN_B0
35
//#define  SW1      PIN_A2      // Motory On/off
36
 
37
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
38
unsigned int8 line;           // na ktere strane byla detekovana cara
39
//unsigned int8 dira;           // pocita dobu po kterou je ztracena cara
40
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
41
//unsigned int8 speed;          // maximalni povolena rychlost
42
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky
43
 
44
signed int16  Lmotor;         // promene, ktere urcuji velikost vykonu na levem
45
signed int16  Rmotor;         // a pravem motoru
46
 
47
// makro pro PWM pro motory
48
#define GO(motor, direction, power) if(get_timer0()<=power) \
49
{direction##motor;} else {stop##motor;}
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
#int_TIMER2
62
TIMER2_isr()      // ovladani serva
63
{
64
   unsigned int8 n;
65
 
66
   output_high(SERVO);
67
   delay_us(1000);
68
   for(n=uhel; n>0; n--) Delay_us(2);
69
   output_low(SERVO);
70
}
71
 
72
////////////////////////////////////////////////////////////////////////////////
73
short int IRcheck()                 // potvrdi detekci cihly
74
{
75
   output_high(IRTX);               // vypne vysilac IR
76
   delay_ms(100);
77
 
78
   output_low(STROBE);
79
   sensors = spi_read(0);         // cteni senzoru
80
   sensors=~sensors;
81
   output_high(STROBE);
82
 
83
   if(true==bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal
84
   {
85
      output_low(IRTX);             // zapne vysilac IR
86
      delay_ms(100);
87
 
88
      output_low(STROBE);
89
      sensors = spi_read(0);         // cteni senzoru
90
      sensors=~sensors;
91
      output_high(STROBE);
92
 
93
      if(false==bit_test(sensors,7))      // otestuje, jestli je detekovana cihla
94
      {
95
         output_high(IRTX);            // vypne vysilac IR
96
         delay_ms(100);
97
 
98
         output_low(STROBE);
99
         sensors = spi_read(0);         // cteni senzoru
100
         sensors=~sensors;
101
         output_high(STROBE);
102
 
103
         output_low(IRTX);             // zapne vysilac IR
104
         if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla
105
      }
106
   };
107
   output_low(IRTX);             // zapne vysilac IR
108
   return 0; // vrat 0, kdyz je detekovano ruseni
109
}
110
////////////////////////////////////////////////////////////////////////////////
111
void objizdka()
112
{
113
   int8 shure=0;
114
   unsigned int16 n;
115
 
116
   BR;BL;
117
   Delay_ms(400);
118
   STOPR;STOPL;
119
 
120
   uhel=KOLMO1;      // nastav zataceci kolecko kolmo na osu robota
121
   Delay_ms(100);
122
   BL;FR;
123
   Delay_ms(200);    // minimalni toceni, kdyby se zastavilo sikmo k cihle
124
 
125
   While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru
126
   {
127
       sensors = spi_read(0);         // cteni senzoru
128
       sensors=~sensors;
129
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
130
   }
131
   STOPL; STOPR;
132
 
133
   for (n=0;n<1500;n++)    // vystred se na hranu cihly
134
   {
135
      if(!input(CIHLA)) {BL; FR;} else {FL; BR;};
136
      delay_ms(1);
137
   }
138
   STOPR;STOPL;
139
 
140
   uhel=STRED;          // dopredu
141
   delay_ms(100);   
142
   FR; FL;
143
   delay_ms(500);
144
   BL;BR;
145
   delay_ms(200);
146
   STOPL;STOPR;
147
 
148
   uhel=STRED+BEAR3;    // doprava
149
   delay_ms(100);   
150
   FL;
151
   delay_ms(200);
152
   uhel=STRED+BEAR2;    // min doprava
153
   FL;FR;
154
   delay_ms(200);
155
   uhel=STRED+BEAR1;    // jeste min doprava
156
   FL;FR;
157
   delay_ms(200);
158
   uhel=STRED;          // rovne
159
   FL;FR;
160
   delay_ms(100);
161
   While((sensors & 0b01111111)!=0) //dokud neni cara
162
   {
163
       sensors = spi_read(0);         // cteni senzoru
164
       sensors=~sensors;
165
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
166
   }
167
   BL; BR;
168
   delay_ms(400);
169
 
170
   uhel=STRED-BEAR3;    // doleva   
171
}
172
 
173
////////////////////////////////////////////////////////////////////////////////
174
void main()
175
{
176
 
177
   unsigned int16 n;
178
   unsigned int8 i;
179
 
180
   setup_adc_ports(NO_ANALOGS);
181
   setup_adc(ADC_CLOCK_INTERNAL);
182
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
183
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
184
   setup_timer_1(T1_DISABLED);
185
   setup_timer_2(T2_DIV_BY_16,140,16);
186
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
187
 
188
   STOPR; STOPL;  // zastav motory
189
   Lmotor=0;Rmotor=0;
190
 
191
   uhel=STRED;    // nastav zadni kolecko na stred
192
   rovinka=0;
193
 
194
   enable_interrupts(INT_TIMER2);
195
   enable_interrupts(GLOBAL);
196
 
197
   output_low(IRTX); // zapni IR vysilac
198
 
199
   delay_ms(1000);
200
 
201
   while(true)
202
   {
203
 
204
      GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory
205
 
206
      delay_us(1500);
207
 
208
      output_low(STROBE);
209
      sensors = spi_read(0);         // cteni senzoru
210
      sensors=~sensors;
211
      output_high(STROBE);
212
 
213
      i=0;                    // havarijni kod
214
      for (n=0; n<=6; n++)
215
      {
216
         if(bit_test(sensors,n)) i++;
217
      }
218
      if (i>3) While(true){STOPR; STOPL;}; // zastavi, kdyz je cerno pod vice nez tremi cidly
219
 
220
      if(bit_test(sensors,7))    // detekce dihly
221
      {
222
         objizdka();
223
      }
224
 
225
      if(bit_test(sensors,3)) //...|...//
226
      {
227
         uhel=STRED;
228
         Lmotor=SPEEDMAX;
229
         Rmotor=SPEEDMAX;
230
         line=S;
231
         if (rovinka<255) rovinka++;
232
         continue;
233
      }
234
 
235
      if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
236
      {
237
         uhel=STRED-BEAR3;
238
         Lmotor=0;
239
         Rmotor=SPEEDMAX;
240
         line=L;
241
         continue;
242
      }
243
 
244
      if(bit_test(sensors,6)) //......|//
245
      {
246
         uhel=STRED+BEAR3;
247
         Rmotor=0;
248
         Lmotor=SPEEDMAX;
249
         line=R;
250
         continue;
251
      }
252
 
253
      if(bit_test(sensors,1)) //.|.....//
254
      {
255
         uhel=STRED-BEAR2;
256
         Lmotor=SPEEDMAX-50;
257
         Rmotor=SPEEDMAX;
258
         line=S;
259
         continue;
260
      }
261
 
262
      if(bit_test(sensors,5)) //.....|.//
263
      {
264
         uhel=STRED+BEAR2;
265
         Rmotor=SPEEDMAX-50;
266
         Lmotor=SPEEDMAX;
267
         line=S;
268
         continue;
269
      }
270
 
271
      if (bit_test(sensors,2)) //..|....//
272
      {
273
         uhel=STRED-BEAR1;
274
         Lmotor=SPEEDMAX;
275
         Rmotor=SPEEDMAX;
276
         line=S;
277
         if (rovinka<255) rovinka++;
278
         continue;
279
      }
280
 
281
      if (bit_test(sensors,4)) //....|..//
282
      {
283
         uhel=STRED+BEAR1;
284
         Rmotor=SPEEDMAX;
285
         Lmotor=SPEEDMAX;
286
         line=S;
287
         if (rovinka<255) rovinka++;
288
         continue;
289
      }
290
 
291
      if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
292
      {
293
         if (rovinka>250)
294
         {
295
            BL; BR;
296
            Delay_ms(100);
297
         };
298
         rovinka=0;
299
      };
300
 
301
   }
302
}