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