Subversion Repositories svnkaklik

Rev

Rev 214 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 214 Rev 215
1
//********* Robot Camerus pro IstRobot 2007 ************
1
//********* Robot Camerus pro IstRobot 2007 ************
2
//"$Id: camerus.c 214 2007-03-23 14:55:54Z kakl $"
2
//"$Id: camerus.c 215 2007-03-24 21:17:01Z kakl $"
3
//*****************************************************
3
//*****************************************************
4
 
4
 
5
#include ".\camerus.h"
5
#include ".\camerus.h"
6
 
6
 
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
7
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
8
 
8
 
9
// A/D vstupy
9
// A/D vstupy
10
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
10
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
11
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
11
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
12
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
12
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
13
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
13
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
14
#define  MODRA    0  // AN0/RA1 - modre kroutitko
14
#define  MODRA    0  // AN0/RA1 - modre kroutitko
15
 
15
 
16
// I/O
16
// I/O
17
#define HREF      PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
17
#define HREF      PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
18
#define PIX       PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
18
#define PIX       PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
19
#define SERVO     PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
19
#define SERVO     PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
20
#define MOT_L     PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
20
#define MOT_L     PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
21
#define MOT_R     PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
21
#define MOT_R     PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
22
#define MOT_1     PIN_C1      // PWM vystpy motoru
22
#define MOT_1     PIN_C1      // PWM vystpy motoru
23
#define MOT_2     PIN_C2      //
23
#define MOT_2     PIN_C2      //
24
#define DATA      PIN_B2      // K modulu LEDbar data
24
#define DATA      PIN_B2      // K modulu LEDbar data
25
#define CP        PIN_B1      // K modulu LEDbar hodiny
25
#define CP        PIN_B1      // K modulu LEDbar hodiny
26
//#define ODO       PIN_A4      // Ze snimace z odometrie z praveho kola / nove je to na RC0 na TIMER1
26
//#define ODO       PIN_A4      // Ze snimace z odometrie z praveho kola / nove je to na RC0 na TIMER1
27
                                // Jeden impuls je 31,25mm
27
                                // Jeden impuls je 31,25mm
28
#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
28
#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
29
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
29
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
30
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
30
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
31
 
31
 
32
#define CASMIN 6           // Rozsah radku snimace
32
#define CASMIN 6           // Rozsah radku snimace
33
#define CASMAX 192
33
#define CASMAX 192
34
#define CASAVR ((CASMAX+CASMIN) / 2)
34
#define CASAVR ((CASMAX+CASMIN) / 2)
35
 
35
 
36
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
36
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
37
 
37
 
-
 
38
#define TRESHOLD 128
-
 
39
 
38
#byte INTCON      = 0x0B         // Interrupt configuration register
40
#byte INTCON      = 0x0B         // Interrupt configuration register
39
   #bit GIE       = INTCON.7
41
   #bit GIE       = INTCON.7
40
   #bit PEIE      = INTCON.6
42
   #bit PEIE      = INTCON.6
41
   #bit TMR0IE    = INTCON.5
43
   #bit TMR0IE    = INTCON.5
42
   #bit INT0IE    = INTCON.4
44
   #bit INT0IE    = INTCON.4
43
   #bit RBIE      = INTCON.3
45
   #bit RBIE      = INTCON.3
44
   #bit TMR0IF    = INTCON.2
46
   #bit TMR0IF    = INTCON.2
45
   #bit INT0IF    = INTCON.1
47
   #bit INT0IF    = INTCON.1
46
   #bit RBIF      = INTCON.0
48
   #bit RBIF      = INTCON.0
47
 
49
 
48
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
50
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
49
stavy stav;       // Kde jsme na trati
51
stavy stav;       // Kde jsme na trati
50
int8 cas;         // Cas hrany bila/cerna v radce
52
int8 cas;         // Cas hrany bila/cerna v radce
51
int8 stred;       // Vystredeni kolecka
53
int8 stred;       // Vystredeni kolecka
52
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
54
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
53
int8 rr;       // Promenna na ulozeni Rozumne rychlost
55
int8 rr;       // Promenna na ulozeni Rozumne rychlost
54
int8 rrold;
56
int8 rrold;
55
 
57
 
56
// Zobrazeni jednoho byte na modulu LEDbar
58
// Zobrazeni jednoho byte na modulu LEDbar
57
inline void disp(int8 x)
59
inline void disp(int8 x)
58
{
60
{
59
   int n;
61
   int n;
60
 
62
 
61
   for(n=0;n<=7;n++)
63
   for(n=0;n<=7;n++)
62
   {
64
   {
63
      if (x & 1 == 1) output_low(DATA); else output_high(DATA);
65
      if (x & 1 == 1) output_low(DATA); else output_high(DATA);
64
      output_high(CP);
66
      output_high(CP);
65
      x>>=1;
67
      x>>=1;
66
      output_low(CP);
68
      output_low(CP);
67
   }
69
   }
68
}
70
}
69
 
71
 
70
// Blikani LEDbarem ve stilu Night Rider
72
// Blikani LEDbarem ve stilu Night Rider
71
void NightRider(int8 x)
73
void NightRider(int8 x)
72
{
74
{
73
   int n,i,j;
75
   int n,i,j;
74
 
76
 
75
   for(j=0;j<x;j++)
77
   for(j=0;j<x;j++)
76
   {
78
   {
77
      i=0x01;
79
      i=0x01;
78
      for(n=0;n<7;n++)
80
      for(n=0;n<7;n++)
79
      {
81
      {
80
         disp(i);
82
         disp(i);
81
         rotate_left(&i, 1);
83
         rotate_left(&i, 1);
82
         delay_ms(40);
84
         delay_ms(40);
83
      }
85
      }
84
      for(n=0;n<7;n++)
86
      for(n=0;n<7;n++)
85
      {
87
      {
86
         disp(i);
88
         disp(i);
87
         rotate_right(&i, 1);
89
         rotate_right(&i, 1);
88
         delay_ms(40);
90
         delay_ms(40);
89
      }
91
      }
90
   }
92
   }
91
   disp(i);
93
   disp(i);
92
   delay_ms(40);
94
   delay_ms(40);
93
   i=0;
95
   i=0;
94
   disp(i);
96
   disp(i);
95
}
97
}
96
 
98
 
97
// Brzdeni motorama stridou 1:1
99
// Brzdeni motorama stridou 1:1
98
void brzda()
100
void brzda()
99
{
101
{
100
   int8 n,i;
102
   int8 n,i;
101
 
103
 
102
   set_pwm1_duty(0);    // vypni PWM
104
   set_pwm1_duty(0);    // vypni PWM
103
   set_pwm2_duty(0);
105
   set_pwm2_duty(0);
104
   setup_ccp1(CCP_OFF);
106
   setup_ccp1(CCP_OFF);
105
   setup_ccp2(CCP_OFF);
107
   setup_ccp2(CCP_OFF);
106
   for (n=0;n<200;n++)
108
   for (n=0;n<200;n++)
107
   {
109
   {
108
      output_low(MOT_L);
110
      output_low(MOT_L);
109
      output_low(MOT_R);
111
      output_low(MOT_R);
110
      output_high(MOT_1);
112
      output_high(MOT_1);
111
      output_high(MOT_2);
113
      output_high(MOT_2);
112
      delay_us(200);
114
      delay_us(200);
113
      output_high(MOT_L);
115
      output_high(MOT_L);
114
      output_high(MOT_R);
116
      output_high(MOT_R);
115
      output_low(MOT_1);
117
      output_low(MOT_1);
116
      output_low(MOT_2);
118
      output_low(MOT_2);
117
      delay_us(200);
119
      delay_us(200);
118
   }
120
   }
119
   output_low(MOT_L); // smer vpred
121
   output_low(MOT_L); // smer vpred
120
   output_low(MOT_R);
122
   output_low(MOT_R);
121
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
123
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
122
   setup_ccp2(CCP_PWM); // RC2
124
   setup_ccp2(CCP_PWM); // RC2
123
}
125
}
124
 
126
 
125
void SetServo(int8 angle)
127
void SetServo(int8 angle)
126
{
128
{
127
   int8 n;
129
   int8 n;
128
 
130
 
129
   for(n=0; n<14; n++)
131
   for(n=0; n<14; n++)
130
   {
132
   {
131
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
133
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
132
      delay_us(1000);
134
      delay_us(1000);
133
      delay_us(stred);
135
      delay_us(stred);
134
      delay_us(stred);
136
      delay_us(stred);
135
      delay_us(stred);
137
      delay_us(stred);
136
      delay_us(angle);
138
      delay_us(angle);
137
      delay_us(angle);
139
      delay_us(angle);
138
      output_low(SERVO);
140
      output_low(SERVO);
139
      delay_ms(18);
141
      delay_ms(18);
140
   }
142
   }
141
}
143
}
142
 
144
 
143
inline void SetServoQ(int8 angle)
145
inline void SetServoQ(int8 angle)
144
{
146
{
145
   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
147
   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
146
   delay_us(1000);
148
   delay_us(1000);
147
   delay_us(stred);
149
   delay_us(stred);
148
   delay_us(stred);
150
   delay_us(stred);
149
   delay_us(stred);
151
   delay_us(stred);
150
   delay_us(angle);
152
   delay_us(angle);
151
   delay_us(angle);
153
   delay_us(angle);
152
   output_low(SERVO);
154
   output_low(SERVO);
153
}
155
}
154
 
156
 
155
#int_EXT
157
#int_EXT
156
EXT_isr()   // Preruseni od prekazky
158
EXT_isr()   // Preruseni od prekazky
157
{
159
{
158
   set_pwm1_duty(0);    // zabrzdi
160
   set_pwm1_duty(0);    // zabrzdi
159
   set_pwm2_duty(0);
161
   set_pwm2_duty(0);
160
   output_high(MOT_L);
162
   output_high(MOT_L);
161
   output_high(MOT_R);
163
   output_high(MOT_R);
162
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
164
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
163
   output_high(IRTX);   // Vypni LED na detekci prekazky
165
   output_high(IRTX);   // Vypni LED na detekci prekazky
164
   delay_ms(2);
166
   delay_ms(2);
165
   if (!input(IRRX))    // stale nas signal?
167
   if (!input(IRRX))    // stale nas signal?
166
   {
168
   {
167
      output_low(MOT_L); // je odraz -> vpred
169
      output_low(MOT_L); // je odraz -> vpred
168
      output_low(MOT_R);
170
      output_low(MOT_R);
169
      return;
171
      return;
170
   };
172
   };
171
   output_low(IRTX);    // Zapni LED na detekci prekazky
173
   output_low(IRTX);    // Zapni LED na detekci prekazky
172
   delay_ms(10);
174
   delay_ms(10);
173
   if (input(IRRX))     // stale nas signal?
175
   if (input(IRRX))     // stale nas signal?
174
   {
176
   {
175
      output_low(MOT_L); // neni odraz -> vpred
177
      output_low(MOT_L); // neni odraz -> vpred
176
      output_low(MOT_R);
178
      output_low(MOT_R);
177
      return;
179
      return;
178
   };
180
   };
179
 
181
 
180
   SetServo((CASAVR-CASMIN));   // rovne
182
   SetServo((CASAVR-CASMIN));   // rovne
181
   delay_ms(100);
183
   delay_ms(100);
182
   brzda();
184
   brzda();
183
 
185
 
184
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
186
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
185
//   if(stav==cihla) return; // Po druhe nic neobjizdej
187
//   if(stav==cihla) return; // Po druhe nic neobjizdej
186
// Pozor na rozjezd
188
// Pozor na rozjezd
187
 
189
 
188
   if(stav==jizda)   // Objed cihlu
190
   if(stav==jizda)   // Objed cihlu
189
   {
191
   {
190
      #include ".\objizdka_R.c"
192
      #include ".\objizdka_R.c"
191
   }
193
   }
192
}
194
}
193
 
195
 
194
 
196
 
195
//---------------------------------- MAIN --------------------------------------
197
//---------------------------------- MAIN --------------------------------------
196
void main()
198
void main()
197
{
199
{
198
   int8 offset;   // Promena pro ulozeni offsetu
200
   int8 offset;   // Promena pro ulozeni offsetu
199
   int8 r1;       // Rychlost motoru 1
201
   int8 r1;       // Rychlost motoru 1
200
   int8 r2;       // Rychlost motoru 2
202
   int8 r2;       // Rychlost motoru 2
201
 
203
 
202
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
204
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
203
   setup_adc(ADC_CLOCK_INTERNAL);
205
   setup_adc(ADC_CLOCK_INTERNAL);
204
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
206
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
205
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
207
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
206
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
208
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
207
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
209
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
208
   setup_ccp2(CCP_PWM); // RC2
210
   setup_ccp2(CCP_PWM); // RC2
209
   setup_comparator(NC_NC_NC_NC);
211
   setup_comparator(NC_NC_NC_NC);
210
   setup_vref(FALSE);
212
   setup_vref(FALSE);
211
 
213
 
212
   set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
214
   set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
213
 
215
 
214
   set_pwm1_duty(0);    // Zastav motory
216
   set_pwm1_duty(0);    // Zastav motory
215
   set_pwm2_duty(0);
217
   set_pwm2_duty(0);
216
   output_low(MOT_L);   // Nastav smer vpred
218
   output_low(MOT_L);   // Nastav smer vpred
217
   output_low(MOT_R);
219
   output_low(MOT_R);
218
 
220
 
219
   output_low(IRTX);   // Zapni LED na detekci prekazky
221
   output_low(IRTX);   // Zapni LED na detekci prekazky
220
 
222
 
221
   NightRider(1);    // Aby se poznalo, ze byl RESET
223
   NightRider(1);    // Aby se poznalo, ze byl RESET
222
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
224
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
223
 
225
 
224
   //... Nastaveni kamery ...
226
   //... Nastaveni kamery ...
225
   i2c_start();      // Soft RESET kamery
227
   i2c_start();      // Soft RESET kamery
226
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
228
   i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
227
   i2c_write(0x12);        // Adresa registru COMH
229
   i2c_write(0x12);        // Adresa registru COMH
228
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
230
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
229
   i2c_stop();
231
   i2c_stop();
230
 
232
 
231
   i2c_start();      // BW
233
   i2c_start();      // BW
232
   i2c_write(0xC0);
234
   i2c_write(0xC0);
233
   i2c_write(0x28);
235
   i2c_write(0x28);
234
   i2c_write(0b01000001);
236
   i2c_write(0b01000001);
235
   i2c_stop();
237
   i2c_stop();
236
 
238
 
237
/*
239
/*
238
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
240
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
239
   i2c_write(0xC0);
241
   i2c_write(0xC0);
240
   i2c_write(0x05);
242
   i2c_write(0x05);
241
   i2c_write(0xA0);  // 48h
243
   i2c_write(0xA0);  // 48h
242
   i2c_stop();
244
   i2c_stop();
243
 
245
 
244
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
246
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
245
   i2c_write(0xC0);
247
   i2c_write(0xC0);
246
   i2c_write(0x2D);
248
   i2c_write(0x2D);
247
   i2c_write(0x04 | 0x03);
249
   i2c_write(0x04 | 0x03);
248
   i2c_stop();
250
   i2c_stop();
249
*/
251
*/
250
 
252
 
251
   i2c_start();      // Fame Rate
253
   i2c_start();      // Fame Rate
252
   i2c_write(0xC0);
254
   i2c_write(0xC0);
253
   i2c_write(0x2B);
255
   i2c_write(0x2B);
254
   i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
256
   i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
255
   i2c_stop();
257
   i2c_stop();
256
 
258
 
257
   i2c_start();      // VSTRT
259
   i2c_start();      // VSTRT
258
   i2c_write(0xC0);
260
   i2c_write(0xC0);
259
   i2c_write(0x19);
261
   i2c_write(0x19);
260
   i2c_write(118);   // prostredni radka
262
   i2c_write(118);   // prostredni radka
261
   i2c_stop();
263
   i2c_stop();
262
 
264
 
263
   i2c_start();      // VEND
265
   i2c_start();      // VEND
264
   i2c_write(0xC0);
266
   i2c_write(0xC0);
265
   i2c_write(0x1A);
267
   i2c_write(0x1A);
266
   i2c_write(118);
268
   i2c_write(118);
267
   i2c_stop();
269
   i2c_stop();
268
 
270
 
269
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
271
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
270
 
272
 
271
   { // Mereni expozice
273
   { // Mereni expozice
272
      int8 t1,t2;
274
      int8 t1,t2;
273
 
275
 
274
      i2c_start();      // Brightness, zacni od uplne tmy
276
      i2c_start();      // Brightness, zacni od uplne tmy
275
      i2c_write(0xC0);
277
      i2c_write(0xC0);
276
      i2c_write(0x06);
278
      i2c_write(0x06);
277
      i2c_write(0);  // 80h default
279
      i2c_write(0);  // 80h default
278
      i2c_stop();
280
      i2c_stop();
279
      delay_ms(50);
281
      delay_ms(50);
280
 
282
 
281
      for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
283
      for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
282
      {
284
      {
283
         i2c_start();      // Brightness
285
         i2c_start();      // Brightness
284
         i2c_write(0xC0);
286
         i2c_write(0xC0);
285
         i2c_write(0x06);
287
         i2c_write(0x06);
286
         i2c_write(offset);  // 80h default
288
         i2c_write(offset);  // 80h default
287
         i2c_stop();
289
         i2c_stop();
288
         disp(offset);
290
         disp(offset);
289
         delay_ms(50);
291
         delay_ms(50);
290
 
292
 
291
         t1=0;
293
         t1=0;
292
         t2=0;
294
         t2=0;
293
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
295
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
294
         delay_ms(5);
296
         delay_ms(5);
295
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
297
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
296
         set_timer0(0);          // Vynuluj pocitadlo casu
298
         set_timer0(0);          // Vynuluj pocitadlo casu
297
         if(!input(PIX)) continue;
299
         if(!input(PIX)) continue;
298
         while(input(PIX));
300
         while(input(PIX));
299
         t1=get_timer0();    // Precti cas z citace casu hrany
301
         t1=get_timer0();    // Precti cas z citace casu hrany
300
         set_timer0(0);          // Vynuluj pocitadlo casu
302
         set_timer0(0);          // Vynuluj pocitadlo casu
301
         while(!input(PIX));
303
         while(!input(PIX));
302
         t2=get_timer0();
304
         t2=get_timer0();
303
 
305
 
304
         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
306
         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
305
 
307
 
306
         delay_ms(2);   // Preskoc druhou radku z kamery
308
         delay_ms(2);   // Preskoc druhou radku z kamery
307
      };
309
      };
308
      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
310
      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
309
   }
311
   }
310
 
312
 
311
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
313
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
312
   delay_ms(1);
314
   delay_ms(1);
313
   offset=read_adc();
315
   offset=read_adc();
314
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
316
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
315
//   offset += 0x70;         // Jas nebude nikdy nizsi
317
//   offset += 0x70;         // Jas nebude nikdy nizsi
316
   disp(offset);
318
   disp(offset);
317
   i2c_start();            // Brightness
319
   i2c_start();            // Brightness
318
   i2c_write(0xC0);
320
   i2c_write(0xC0);
319
   i2c_write(0x06);
321
   i2c_write(0x06);
320
   i2c_write(offset);  // 80h default
322
   i2c_write(offset);  // 80h default
321
   i2c_stop();
323
   i2c_stop();
322
   delay_ms(1000);   // Nech hodnotu chvili na displayi
324
   delay_ms(1000);   // Nech hodnotu chvili na displayi
323
 
325
 
324
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
326
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
325
   delay_ms(1);
327
   delay_ms(1);
326
   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
328
   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
327
   rrold=rr;
329
   rrold=rr;
328
 
330
 
329
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
331
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
330
                        // a aby se to rozjelo jeste dneska
332
                        // a aby se to rozjelo jeste dneska
331
   stav=start;          // Jsme na startu
333
   stav=start;          // Jsme na startu
332
   set_timer1(0);       // Vynuluj citac odometrie
334
   set_timer1(0);       // Vynuluj citac odometrie
333
 
335
 
334
   // ........................... Hlavni smycka ................................
336
   // ........................... Hlavni smycka ................................
335
   while(true)
337
   while(true)
336
   {
338
   {
337
      int8 pom;
339
      int8 pom;
338
      int8 n;
340
      int8 n;
339
 
341
 
340
next_snap:
342
next_snap:
341
 
343
 
342
      pom=0;
344
      pom=0;
343
      disable_interrupts(GLOBAL);  //----------------------- Critical
345
      disable_interrupts(GLOBAL);  //----------------------- Critical
344
      while(input(HREF));     // Preskoc 1. radku
346
      while(input(HREF));     // Preskoc 1. radku
345
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
347
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
346
      set_timer0(0);          // Vynuluj pocitadlo casu
348
      set_timer0(0);          // Vynuluj pocitadlo casu
347
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
349
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
348
      {
350
      {
349
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
351
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
350
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
352
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
351
         if(!input(PIX))
353
         if(!input(PIX))
352
//         if(!input(PIX))
354
//         if(!input(PIX))
353
         {
355
         {
354
            pom=get_timer0();    // Precti cas z citace casu hrany
356
            pom=get_timer0();    // Precti cas z citace casu hrany
355
            break;
357
            break;
356
         };
358
         };
357
      };
359
      };
358
      while(input(HREF));      // Pockej na shozeni signalu HREF
360
      while(input(HREF));      // Pockej na shozeni signalu HREF
359
 
361
 
360
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
362
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
361
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
363
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
362
      // a vubec to nejak na kraji nefunguje.
364
      // a vubec to nejak na kraji nefunguje.
363
 
365
 
364
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
366
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
365
      delay_us(1000);
367
      delay_us(1000);
366
      delay_us(stred);
368
      delay_us(stred);
367
      delay_us(stred);
369
      delay_us(stred);
368
      delay_us(stred);
370
      delay_us(stred);
369
      delay_us(cas);
371
      delay_us(cas);
370
      delay_us(cas);
372
      delay_us(cas);
371
      output_low(SERVO);
373
      output_low(SERVO);
372
 
374
 
373
      // Elektronicky diferencial
375
      // Elektronicky diferencial
374
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
376
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
375
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
377
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
376
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
378
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
377
 
379
 
378
      enable_interrupts(GLOBAL);    //----------------------- Critical
380
      enable_interrupts(GLOBAL);    //----------------------- Critical
379
 
381
 
380
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
382
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
381
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
383
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
382
 
384
 
383
//      r1<<=1;     // Rychlost je dvojnasobna
385
//      r1<<=1;     // Rychlost je dvojnasobna
384
//      r2<<=1;     // Rozsah 2 az 184
386
//      r2<<=1;     // Rozsah 2 az 184
385
 
387
 
386
/* Nerozumna rychlost po cihle
388
/* Nerozumna rychlost po cihle
387
      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
389
      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
388
      {
390
      {
389
         rr=rrold;
391
         rr=rrold;
390
         stav=pocihle;
392
         stav=pocihle;
391
      };
393
      };
392
*/
394
*/
393
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
395
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
394
      {
396
      {
395
         set_pwm1_duty(r1);
397
         set_pwm1_duty(r1);
396
         set_pwm2_duty(r2);
398
         set_pwm2_duty(r2);
397
      }
399
      }
398
      else
400
      else
399
      {
401
      {
400
         set_pwm1_duty(0);       // Zastaveni
402
         set_pwm1_duty(0);       // Zastaveni
401
         set_pwm2_duty(0);
403
         set_pwm2_duty(0);
402
      };
404
      };
403
 
405
 
404
      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
406
      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
405
      {
407
      {
406
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
408
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
407
         INT0IF=0;                     // Zruseni predesle udalosti od startera
409
         INT0IF=0;                     // Zruseni predesle udalosti od startera
408
         enable_interrupts(INT_EXT);
410
         enable_interrupts(INT_EXT);
409
         enable_interrupts(GLOBAL);
411
         enable_interrupts(GLOBAL);
410
         stav=jizda;
412
         stav=jizda;
411
      };
413
      };
412
 
414
 
413
      if(stav==start)         // Snimkuje, toci servem a ceka na start
415
      if(stav==start)         // Snimkuje, toci servem a ceka na start
414
      {
416
      {
415
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
417
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
416
         Delay_ms(1);
418
         Delay_ms(1);
417
         stred=read_adc();
419
         stred=read_adc();
418
         if(!input(PROXIMITY))
420
         if(!input(PROXIMITY))
419
         {
421
         {
420
            disp(0x80);
422
            disp(0x80);
421
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
423
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
422
            set_timer1(0);         // Vynuluj citac odometrie
424
            set_timer1(0);         // Vynuluj citac odometrie
423
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
425
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
424
            set_pwm2_duty(255);
426
            set_pwm2_duty(255);
425
            disp(0x01);
427
            disp(0x01);
426
            while(get_timer1()<=4) // Ujed alespon 12cm
428
            while(get_timer1()<=4) // Ujed alespon 12cm
427
            {
429
            {
428
               set_adc_channel(LMAX);    // Levy UV sensor
430
               set_adc_channel(LMAX);    // Levy UV sensor
429
               delay_us(40);
431
               delay_us(40);
430
               if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
432
               if(read_adc()<TRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
431
               set_adc_channel(RMAX);    // Pravy UV sensor
433
               set_adc_channel(RMAX);    // Pravy UV sensor
432
               delay_us(40);
434
               delay_us(40);
433
               if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
435
               if(read_adc()<TRESHOLD) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
434
               cas=CASAVR-CASMIN;    // Cara je rovne
436
               cas=CASAVR-CASMIN;    // Cara je rovne
435
            };
437
            };
436
            stav=rozjezd;
438
            stav=rozjezd;
437
         };
439
         };
438
      }
440
      }
439
 
441
 
440
      pom=0x80;    // Zobrazeni pozice cary na displayi
442
      pom=0x80;    // Zobrazeni pozice cary na displayi
441
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
443
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
442
      disp(pom);
444
      disp(pom);
443
 
445
 
444
      while(true) // Ve zbytku casu snimku cti krajni UV senzory
446
      while(true) // Ve zbytku casu snimku cti krajni UV senzory
445
      {
447
      {
446
         set_adc_channel(LMAX);    // Levy UV sensor
448
         set_adc_channel(LMAX);    // Levy UV sensor
447
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
449
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
448
         if(read_adc()<128) cas=CASMIN;
450
         if(read_adc()<TRESHOLD) cas=CASMIN;
449
         set_adc_channel(RMAX);    // Pravy UV sensor
451
         set_adc_channel(RMAX);    // Pravy UV sensor
450
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
452
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
451
         if(read_adc()<128) cas=CASMAX;
453
         if(read_adc()<TRESHOLD) cas=CASMAX;
452
      };
454
      };
453
   }
455
   }
454
}
456
}