Subversion Repositories svnkaklik

Rev

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

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