Subversion Repositories svnkaklik

Rev

Go to most recent revision | 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       10//10          // 3 stupne zataceni
7
#define  BEAR2       25//25
8
#define  BEAR3       45//45
9
#define  R           100         // Rozumna rychlost
10
#define  R17         200         // X nasobek rozumne rychlosti
11
#define  PRED_CIHLOU 100         // rychlost pri dalkove detekci cihly
12
//#define  L1            1     // cara vlevo
13
#define  L2            2     // cara vlevo
14
#define  L3            3     // cara vlevo
15
#define  S             0     // cara mezi sensory
16
//#define  R1           -1     // cara vpravo
17
#define  R2           -2     // cara vpravo
18
#define  R3           -3     // cara vpravo
19
 
20
// servo
21
#define  SERVO PIN_B5
22
 
23
// kroutitka
24
#define  CERVENA  4  // AN4
25
//#define  CERNA    5  // AN5
26
//#define  ZELENA   6  // AN6
27
#define  MODRA    2  // AN2
28
 
29
// IR
30
#define IRTX      PIN_B2
31
#define  CIHLA    PIN_A3
32
 
33
//motory
34
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
35
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
36
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
37
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
38
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
39
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)
40
 
41
//HID
42
#define  LED1     PIN_B1      //oranzova
43
#define  LED2     PIN_B2      //zluta
44
 
45
#define  STROBE   PIN_B0
46
//#define  SW1      PIN_A2      // Motory On/off
47
 
48
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
49
signed int8 line = S;         // na ktere strane byla detekovana cara
50
//unsigned int8 dira;         // pocita dobu po kterou je ztracena cara
51
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
52
unsigned int8 speed;          // maximalni povolena rychlost
53
unsigned int8 turn;           // rychlost toceni
54
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky
55
 
56
signed int16  Lmotor;         // promene, ktere urcuji velikost vykonu na levem
57
signed int16  Rmotor;         // a pravem motoru
58
 
59
// makro pro PWM pro motory
60
#define GO(motor, direction, power) if(get_timer0()<=power) \
61
{direction##motor;} else {stop##motor;}
62
////////////////////////////////////////////////////////////////////////////////
63
#int_TIMER2
64
TIMER2_isr()      // ovladani serva
65
{
66
   unsigned int8 n;
67
 
68
   output_high(SERVO);
69
   delay_us(1000);
70
   for(n=uhel; n>0; n--) Delay_us(2);
71
   output_low(SERVO);
72
}
73
 
74
////////////////////////////////////////////////////////////////////////////////
75
int8 IRcheck()                 // potvrdi detekci cihly
76
{
77
   output_high(IRTX);               // vypne vysilac IR
78
   delay_ms(10);
79
 
80
   output_low(STROBE);
81
   sensors = spi_read(0);         // cteni senzoru
82
   sensors=~sensors;
83
   output_high(STROBE);
84
 
85
   if(bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal
86
   {
87
      output_low(IRTX);             // zapne vysilac IR
88
      delay_ms(10);
89
 
90
      output_low(STROBE);
91
      sensors = spi_read(0);         // cteni senzoru
92
      sensors=~sensors;
93
      output_high(STROBE);
94
 
95
      if(!bit_test(sensors,7))       // otestuje, jestli je detekovana cihla
96
      {
97
         output_high(IRTX);            // vypne vysilac IR
98
         delay_ms(10);
99
 
100
         output_low(STROBE);
101
         sensors = spi_read(0);         // cteni senzoru
102
         sensors=~sensors;
103
         output_high(STROBE);
104
 
105
         output_low(IRTX);             // zapne vysilac IR
106
 
107
         if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla
108
      }
109
   };
110
   output_low(IRTX);             // zapne vysilac IR
111
   return 0; // vrat 0, kdyz je detekovano ruseni
112
}
113
////////////////////////////////////////////////////////////////////////////////
114
#include ".\objizdka_centrovani.c"
115
////////////////////////////////////////////////////////////////////////////////
116
void main()
117
{
118
 
119
   unsigned int8 n;
120
   unsigned int8 i,j;
121
   unsigned int8 last_sensors;
122
   unsigned int8 RozumnaRychlost;
123
 
124
   setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka
125
   setup_adc(ADC_CLOCK_INTERNAL);
126
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16);
127
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
128
   setup_timer_1(T1_DISABLED|T1_DIV_BY_8);
129
   setup_timer_2(T2_DIV_BY_16,140,16);
130
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
131
 
132
   STOPR; STOPL;  // zastav motory
133
   Lmotor=0;Rmotor=0;
134
 
135
   uhel = STRED;    // nastav zadni kolecko na stred
136
   rovinka = 0;
137
 
138
   enable_interrupts(INT_TIMER2);
139
   enable_interrupts(GLOBAL);
140
 
141
   output_low(IRTX); // zapni IR vysilac
142
 
143
   delay_ms(2000); // musime pockat na diagnostiku slave CPU
144
 
145
   //nastaveni rychlosti
146
   set_adc_channel(CERVENA);
147
 
148
   Delay_ms(1);
149
   RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63
150
   set_adc_channel(MODRA);
151
   Delay_ms(1);
152
   turn=speed-32+(read_adc()>>2);  // rychlost toceni +-32; kroutitko dava 0-63
153
 
154
   speed=R17;
155
 
156
   while(true)
157
   {
158
 
159
      GO(L,F,Lmotor);GO(R,F,Rmotor);   // zapni motory PWM podle promenych Lmotor a Rmotor
160
 
161
      delay_us(2000);                  // cekani na SLAVE, nez pripravi data od cidel
162
 
163
      last_sensors=sensors;
164
 
165
      output_low(STROBE);              // vypni zobrazovani na posuvnem registru
166
      sensors = spi_read(0);           // cteni senzoru
167
      sensors=~sensors;                // neguj prijata data
168
      output_high(STROBE);       // zobraz data na posuvnem registru
169
 
170
      i=0;                       // havarijni kod
171
      for (n=0; n<=6; n++)
172
      {
173
         if(bit_test(sensors,n)) i++;
174
      }
175
      if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly
176
      {
177
         BL; BR;
178
         delay_ms(300);
179
         STOPR; STOPL;
180
         While(true);
181
      };
182
 
183
/*
184
      if (!input(CIHLA))      // dalkova detekce cihly
185
      {
186
         speed=PRED_CIHLOU;
187
      }
188
      else
189
      {
190
         speed=RozumnaRychlost;
191
      }
192
*/
193
      if (bit_test(sensors,7))    // detekce cihly
194
      {
195
         BR;BL;
196
         Delay_ms(400);
197
         STOPR;STOPL;
198
//         if (1==IRcheck())   // kontrola, jestli nebylo rusene cidlo
199
         {
200
            Delay_ms(100);
201
            cikcak();
202
            delay_ms(100);
203
            objizdka();       // objede cihlu
204
         }
205
      }
206
 
207
      if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl.
208
 
209
      if(bit_test(sensors,3)) //...|...//
210
      {
211
         uhel=STRED;
212
         Lmotor=speed;
213
         Rmotor=speed;
214
         line=S;
215
         if (rovinka < 255) rovinka++;
216
         continue;
217
      }
218
 
219
      if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
220
      {
221
         uhel=STRED - BEAR3;
222
         Lmotor=0;
223
         Rmotor=turn;
224
         line=L3;
225
         continue;
226
      }
227
 
228
      if(bit_test(sensors,6)) //......|//
229
      {
230
         uhel=STRED + BEAR3;
231
         Rmotor=0;
232
         Lmotor=turn;
233
         line=R3;
234
         continue;
235
      }
236
 
237
      if(bit_test(sensors,1)) //.|.....//
238
      {
239
         uhel=STRED - BEAR2;
240
         Lmotor=speed-70;
241
         Rmotor=speed;
242
         line=L2;
243
         continue;
244
      }
245
 
246
      if(bit_test(sensors,5)) //.....|.//
247
      {
248
         uhel=STRED + BEAR2;
249
         Rmotor=speed-70;
250
         Lmotor=speed;
251
         line=R2;
252
         continue;
253
      }
254
 
255
      if (bit_test(sensors,2)) //..|....//
256
      {
257
         uhel=STRED - BEAR1;
258
         Lmotor=speed-20;
259
         Rmotor=speed;
260
         line=L2;
261
         if (rovinka<255) rovinka++;
262
         continue;
263
      }
264
 
265
      if (bit_test(sensors,4)) //....|..//
266
      {
267
         uhel=STRED + BEAR1;
268
         Rmotor=speed-20;
269
         Lmotor=speed;
270
         line=L2;
271
         if (rovinka<255) rovinka++;
272
         continue;
273
      }
274
 
275
 
276
      if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate
277
      {
278
         if (rovinka>50)
279
         {
280
            BL; BR;
281
            Delay_ms(100);
282
            if (rovinka > 250 || speed > 170) delay_ms(50);
283
         };
284
         rovinka=0;
285
         speed=R17;
286
      };
287
   }
288
}