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          // 3 stupne zataceni
7
#define  BEAR2       25
8
#define  BEAR3       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  L2            2     // cara vlevo
13
#define  L3            3     // cara vlevo
14
#define  S             0     // cara mezi sensory
15
#define  R2           -2     // cara vpravo
16
#define  R3           -3     // cara vpravo
17
 
18
// servo
19
#define  SERVO PIN_B5
20
 
21
// kroutitka
22
#define  CERVENA  4  // AN4
23
#define  MODRA    2  // AN2
24
 
25
// IR
26
#define IRTX      PIN_B2
27
#define  CIHLA    PIN_A3
28
 
29
//motory
30
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
31
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
32
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
33
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
34
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
35
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)
36
 
37
//HID
38
#define  LED1     PIN_B1      //oranzova
39
#define  LED2     PIN_B2      //zluta
40
 
41
#define  STROBE   PIN_B0
42
 
43
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
44
signed int8 line = S;         // na ktere strane byla detekovana cara
45
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
46
unsigned int8 speed;          // maximalni povolena rychlost
47
unsigned int8 turn;           // rychlost toceni
48
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky
49
 
50
signed int16  Lmotor;         // promene, ktere urcuji velikost vykonu na levem
51
signed int16  Rmotor;         // a pravem motoru
52
 
53
// makro pro PWM pro motory
54
#define GO(motor, direction, power) if(get_timer0()<=power) \
55
{direction##motor;} else {stop##motor;}
56
////////////////////////////////////////////////////////////////////////////////
57
#int_TIMER2
58
TIMER2_isr()      // ovladani serva
59
{
60
   unsigned int8 n;
61
 
62
   output_high(SERVO);
63
   delay_us(1000);
64
   for(n=uhel; n>0; n--) Delay_us(2);
65
   output_low(SERVO);
66
}
67
 
68
////////////////////////////////////////////////////////////////////////////////
69
void main()
70
{
71
 
72
   unsigned int8 n;
73
   unsigned int8 i,j;
74
   unsigned int8 last_sensors;
75
   unsigned int8 RozumnaRychlost;
76
 
77
   setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka
78
   setup_adc(ADC_CLOCK_INTERNAL);
79
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16);
80
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
81
   setup_timer_1(T1_DISABLED|T1_DIV_BY_8);
82
   setup_timer_2(T2_DIV_BY_16,140,16);
83
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
84
 
85
   STOPR; STOPL;  // zastav motory
86
   Lmotor=0;Rmotor=0;
87
 
88
   uhel = STRED;    // nastav zadni kolecko na stred
89
   rovinka = 0;
90
 
91
   enable_interrupts(INT_TIMER2);
92
   enable_interrupts(GLOBAL);
93
 
94
   output_low(IRTX); // zapni IR vysilac
95
 
96
   delay_ms(2000); // musime pockat na diagnostiku slave CPU
97
 
98
   //nastaveni rychlosti
99
   set_adc_channel(CERVENA);
100
 
101
   Delay_ms(1);
102
   RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63
103
   set_adc_channel(MODRA);
104
   Delay_ms(1);
105
   turn=speed-32+(read_adc()>>2);  // rychlost toceni +-32; kroutitko dava 0-63
106
 
107
 
108
   while(true)
109
   {
110
 
111
      GO(L,F,Lmotor);GO(R,F,Rmotor);   // zapni motory PWM podle promenych Lmotor a Rmotor
112
 
113
      delay_us(2000);                  // cekani na SLAVE, nez pripravi data od cidel
114
 
115
      last_sensors=sensors;
116
 
117
      output_low(STROBE);              // vypni zobrazovani na posuvnem registru
118
      sensors = spi_read(0);           // cteni senzoru
119
      sensors=~sensors;                // neguj prijata data
120
      output_high(STROBE);       // zobraz data na posuvnem registru
121
 
122
      i=0;                       // havarijni kod
123
      for (n=0; n<=6; n++)
124
      {
125
         if(bit_test(sensors,n)) i++;
126
      }
127
      if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly
128
      {
129
         BL; BR;
130
         delay_ms(300);
131
         STOPR; STOPL;
132
         While(true);
133
      };
134
 
135
      if (bit_test(sensors,7))    // detekce cihly
136
      {
137
         BR;BL;
138
         Delay_ms(100);
139
         STOPR;STOPL;
140
         Delay_ms(1000);
141
      }
142
 
143
      if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
144
      {
145
         uhel=STRED - BEAR3;
146
         Lmotor=0;
147
         Rmotor=turn;
148
         line=L3;
149
         continue;
150
      }
151
 
152
      if(bit_test(sensors,6)) //......|//
153
      {
154
         uhel=STRED + BEAR3;
155
         Rmotor=0;
156
         Lmotor=turn;
157
         line=R3;
158
         continue;
159
      }
160
 
161
      if(bit_test(sensors,1)) //.|.....//
162
      {
163
         uhel=STRED - BEAR2;
164
         Lmotor=speed-70;
165
         Rmotor=speed;
166
         line=L2;
167
         continue;
168
      }
169
 
170
      if(bit_test(sensors,5)) //.....|.//
171
      {
172
         uhel=STRED + BEAR2;
173
         Rmotor=speed-70;
174
         Lmotor=speed;
175
         line=R2;
176
         continue;
177
      }
178
 
179
      if (bit_test(sensors,2)) //..|....//
180
      {
181
         uhel=STRED - BEAR1;
182
         Lmotor=speed-20;
183
         Rmotor=speed;
184
         line=L2;
185
         if (rovinka<255) rovinka++;
186
         continue;
187
      }
188
 
189
      if (bit_test(sensors,4)) //....|..//
190
      {
191
         uhel=STRED + BEAR1;
192
         Rmotor=speed-20;
193
         Lmotor=speed;
194
         line=L2;
195
         if (rovinka<255) rovinka++;
196
         continue;
197
      }
198
 
199
      if(bit_test(sensors,3)) //...|...//
200
      {
201
         uhel=STRED;
202
         Lmotor=speed;
203
         Rmotor=speed;
204
         line=S;
205
         if (rovinka < 255) rovinka++;
206
         continue;
207
      }
208
 
209
 
210
 
211
      if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate
212
      {
213
         if (rovinka>50)
214
         {
215
            BL; BR;
216
            Delay_ms(100);
217
            if (rovinka > 250 || speed > 170) delay_ms(50);
218
         };
219
         rovinka=0;
220
      };
221
   }
222
}