Subversion Repositories svnkaklik

Rev

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

Rev 234 Rev 236
1
//********* Robot Camerus pro IstRobot 2007 ************
1
//********* Robot Camerus pro IstRobot 2007 ************
2
//"$Id: camerus.c 234 2007-04-16 21:19:54Z kaklik $"
2
//"$Id: camerus.c 236 2007-04-17 14:40:34Z 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
// Rychlostni konstanty
-
 
10
#define RR_CIHLA     20       // Rozumna rychlost pro objizdeni cihly
-
 
11
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
-
 
12
#define BRZDNA_DRAHA 8        // Jak daleko pred problemem se zacne brzdit
-
 
13
 
9
#define CAMERA_adr   0xC0
14
#define CAMERA_ADR   0xC0
10
#define SONAR_adr    0xE0
15
#define SONAR_ADR    0xE0
11
 
16
 
12
// A/D vstupy
17
// A/D vstupy
13
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
18
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
14
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
19
#define  LMAX     3  // AN3/RA3 - prave cidlo na vyjeti z cary
15
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
20
#define  CERVENA  2  // AN2/RA2 - cervene kroutitko
16
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
21
#define  ZELENA   1  // AN1/RA0 - zelene kroutitko
17
#define  MODRA    0  // AN0/RA1 - modre kroutitko
22
#define  MODRA    0  // AN0/RA1 - modre kroutitko
18
 
23
 
19
// I/O
24
// I/O
20
#define HREF      PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
25
#define HREF      PIN_C5      // Signal HREF z kamery (v H po celou dobu radku)
21
#define PIX       PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
26
#define PIX       PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
22
#define SERVO     PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
27
#define SERVO     PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
23
#define MOT_L     PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
28
#define MOT_L     PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
24
#define MOT_R     PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
29
#define MOT_R     PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
25
#define MOT_1     PIN_C1      // PWM vystpy motoru
30
#define MOT_1     PIN_C1      // PWM vystpy motoru
26
#define MOT_2     PIN_C2      //
31
#define MOT_2     PIN_C2      //
27
#define DATA      PIN_B2      // K modulu LEDbar data
32
#define DATA      PIN_B2      // K modulu LEDbar data
28
#define CP        PIN_B1      // K modulu LEDbar hodiny
33
#define CP        PIN_B1      // K modulu LEDbar hodiny
29
//#define ODO       PIN_C0      // Ze snimace z odometrie z praveho kola na TIMER1
34
//#define ODO       PIN_C0      // Ze snimace z odometrie z praveho kola na TIMER1
30
                                // Jeden impuls je 31,25mm
35
                                // Jeden impuls je 31,25mm
31
#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
36
#define IRRX      !input(PIN_B0)   // Vstup INT, generuje preruseni pri prekazce
32
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
37
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
33
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
38
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
-
 
39
#define BUMPER    !input(PIN_A4)  // Naraznik
34
 
40
 
35
#define CASMIN 6           // Rozsah radku snimace
41
#define CASMIN 6           // Rozsah radku snimace
36
#define CASMAX 192
42
#define CASMAX 192
37
#define CASAVR ((CASMAX+CASMIN) / 2)
43
#define CASAVR ((CASMAX+CASMIN) / 2)
38
 
44
 
-
 
45
#define EEMAX  255         // Konec EEPROM
-
 
46
#define MAXLOG 0x30        // Maximalni pocet zaznamu v logu
-
 
47
#if  MAXLOG>(EEMAX/2-1)
-
 
48
   #error  Prekrocena velikost EEPROM
-
 
49
#endif
-
 
50
 
39
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
51
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
40
 
52
 
41
#define THR 90    // Threshold pro UV cidla na caru
53
#define THR 90    // Threshold pro UV cidla na caru
42
 
54
 
43
#byte INTCON      = 0x0B         // Interrupt configuration register
55
#byte INTCON      = 0x0B         // Interrupt configuration register
44
   #bit GIE       = INTCON.7
56
   #bit GIE       = INTCON.7
45
   #bit PEIE      = INTCON.6
57
   #bit PEIE      = INTCON.6
46
   #bit TMR0IE    = INTCON.5
58
   #bit TMR0IE    = INTCON.5
47
   #bit INT0IE    = INTCON.4
59
   #bit INT0IE    = INTCON.4
48
   #bit RBIE      = INTCON.3
60
   #bit RBIE      = INTCON.3
49
   #bit TMR0IF    = INTCON.2
61
   #bit TMR0IF    = INTCON.2
50
   #bit INT0IF    = INTCON.1
62
   #bit INT0IF    = INTCON.1
51
   #bit RBIF      = INTCON.0
63
   #bit RBIF      = INTCON.0
52
 
64
 
53
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
65
enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
54
stavy stav;       // Kde jsme na trati
66
stavy stav;       // Kde jsme na trati
55
int8 cas;         // Cas hrany bila/cerna v radce
67
int8 cas;         // Cas hrany bila/cerna v radce
56
int8 stred;       // Vystredeni kolecka
68
int8 stred;       // Vystredeni kolecka
57
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
69
int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie
-
 
70
int16 last_log_odo; // Posledni stav odometrie poznamenany do logu
-
 
71
int16 last_log;   // Cislo posledniho zaznamu v logu v EEPROM
-
 
72
int8 bb_h[MAXLOG];  // Cerna skrinka MSB
-
 
73
int8 bb_l[MAXLOG];  // Cerna skrinka LSB
-
 
74
int8 log;         // Pocitadlo pro cernou skrinku
-
 
75
int16 bb;         // Posledni nactena udalost z Black Boxu
-
 
76
int16 timer_pom;  // Pomocna promenna pro atomicke nacteni hodnoty timeru1
58
int8 rr;       // Promenna na ulozeni Rozumne rychlost
77
int8 rr;          // Promenna na ulozeni Rozumne rychlost
59
int8 rrold;
78
int8 rrold;
60
 
79
 
61
// Zobrazeni jednoho byte na modulu LEDbar
80
// Zobrazeni jednoho byte na modulu LEDbar
62
inline void disp(int8 x)
81
inline void disp(int8 x)
63
{
82
{
64
   int n;
83
   int n;
65
 
84
 
66
   for(n=0;n<=7;n++)
85
   for(n=0;n<=7;n++)
67
   {
86
   {
68
      if (x & 1 == 1) output_low(DATA); else output_high(DATA);
87
      if (bit_test(x,0)) output_low(DATA); else output_high(DATA);
69
      output_high(CP);
88
      output_high(CP);
70
      x>>=1;
89
      x>>=1;
71
      output_low(CP);
90
      output_low(CP);
72
   }
91
   }
73
}
92
}
74
 
93
 
75
// Blikani LEDbarem ve stilu Night Rider
94
// Blikani LEDbarem ve stilu Night Rider
76
void NightRider(int8 x)
95
void NightRider(int8 x)
77
{
96
{
78
   int n,i,j;
97
   int n,i,j;
79
 
98
 
80
   for(j=0;j<x;j++)
99
   for(j=0;j<x;j++)
81
   {
100
   {
82
      i=0x01;
101
      i=0x01;
83
      for(n=0;n<7;n++)
102
      for(n=0;n<7;n++)
84
      {
103
      {
85
         disp(i);
104
         disp(i);
86
         rotate_left(&i, 1);
105
         rotate_left(&i, 1);
87
         delay_ms(40);
106
         delay_ms(40);
88
      }
107
      }
89
      for(n=0;n<7;n++)
108
      for(n=0;n<7;n++)
90
      {
109
      {
91
         disp(i);
110
         disp(i);
92
         rotate_right(&i, 1);
111
         rotate_right(&i, 1);
93
         delay_ms(40);
112
         delay_ms(40);
94
      }
113
      }
95
   }
114
   }
96
   disp(i);
115
   disp(i);
97
   delay_ms(40);
116
   delay_ms(40);
98
   i=0;
117
   i=0;
99
   disp(i);
118
   disp(i);
100
}
119
}
101
 
120
 
-
 
121
// Zaznam LOGu do EEPROM
-
 
122
void SaveLog(int8 log)
-
 
123
{
-
 
124
   int8 n;
-
 
125
 
-
 
126
   for(n=0;n<=(log*2);n+=2)   // Ulozeni Black Boxu do EEPROM
-
 
127
   {
-
 
128
      write_eeprom(n,bb_h[n/2]);
-
 
129
      write_eeprom(n+1,bb_l[n/2]);
-
 
130
   };
-
 
131
   write_eeprom(EEMAX,log);  // Zapis poctu zaznamu na konec EEPROM
-
 
132
}
-
 
133
 
102
// Brzdeni motorama stridou 1:1
134
// Brzdeni motorama stridou 1:1
103
void brzda()
135
void brzda()
104
{
136
{
105
   int8 n,i;
137
   int8 n,i;
106
 
138
 
107
   set_pwm1_duty(0);    // vypni PWM
139
   set_pwm1_duty(0);    // vypni PWM
108
   set_pwm2_duty(0);
140
   set_pwm2_duty(0);
109
   setup_ccp1(CCP_OFF);
141
   setup_ccp1(CCP_OFF);
110
   setup_ccp2(CCP_OFF);
142
   setup_ccp2(CCP_OFF);
111
   for (n=0;n<200;n++)
143
   for (n=0;n<200;n++)
112
   {
144
   {
113
      output_low(MOT_L);
145
      output_low(MOT_L);
114
      output_low(MOT_R);
146
      output_low(MOT_R);
115
      output_high(MOT_1);
147
      output_high(MOT_1);
116
      output_high(MOT_2);
148
      output_high(MOT_2);
117
      delay_us(200);
149
      delay_us(200);
118
      output_high(MOT_L);
150
      output_high(MOT_L);
119
      output_high(MOT_R);
151
      output_high(MOT_R);
120
      output_low(MOT_1);
152
      output_low(MOT_1);
121
      output_low(MOT_2);
153
      output_low(MOT_2);
122
      delay_us(200);
154
      delay_us(200);
123
   }
155
   }
124
   output_low(MOT_L); // smer vpred
156
   output_low(MOT_L); // smer vpred
125
   output_low(MOT_R);
157
   output_low(MOT_R);
126
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
158
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
127
   setup_ccp2(CCP_PWM); // RC2
159
   setup_ccp2(CCP_PWM); // RC2
128
}
160
}
129
 
161
 
130
void SetServo(int8 angle)
162
void SetServo(int8 angle)
131
{
163
{
132
   int8 n;
164
   int8 n;
133
 
165
 
134
   for(n=0; n<14; n++)
166
   for(n=0; n<14; n++)
135
   {
167
   {
136
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
168
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
137
      delay_us(1000);
169
      delay_us(1000);
138
      delay_us(stred);
170
      delay_us(stred);
139
      delay_us(stred);
171
      delay_us(stred);
140
      delay_us(stred);
172
      delay_us(stred);
141
      delay_us(angle);
173
      delay_us(angle);
142
      delay_us(angle);
174
      delay_us(angle);
143
      output_low(SERVO);
175
      output_low(SERVO);
144
      delay_ms(18);
176
      delay_ms(18);
145
   }
177
   }
146
}
178
}
147
 
179
 
148
inline void SetServoQ(int8 angle)
180
inline void SetServoQ(int8 angle)
149
{
181
{
150
   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
182
   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
151
   delay_us(1000);
183
   delay_us(1000);
152
   delay_us(stred);
184
   delay_us(stred);
153
   delay_us(stred);
185
   delay_us(stred);
154
   delay_us(stred);
186
   delay_us(stred);
155
   delay_us(angle);
187
   delay_us(angle);
156
   delay_us(angle);
188
   delay_us(angle);
157
   output_low(SERVO);
189
   output_low(SERVO);
158
}
190
}
159
 
191
 
160
//---------------------------- INT --------------------------------
192
//---------------------------- INT --------------------------------
161
#int_EXT
193
#int_EXT
162
EXT_isr()   // Preruseni od prekazky
194
EXT_isr()   // Preruseni od prekazky
163
{
195
{
164
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
196
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
165
   set_pwm2_duty(0);
197
   set_pwm2_duty(0);
166
   output_high(MOT_L);
198
   output_high(MOT_L);
167
   output_low(MOT_R);
199
   output_low(MOT_R);
168
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
200
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
169
   output_high(IRTX);   // Vypni LED na detekci prekazky
201
   output_high(IRTX);   // Vypni LED na detekci prekazky
170
   delay_ms(2);
202
   delay_ms(2);
171
   if (!input(IRRX))    // stale nas signal?
203
   if (IRRX)    // stale nas signal?
172
   {
204
   {
173
      output_low(MOT_L); // je odraz -> vpred
205
      output_low(MOT_L); // je odraz -> vpred
174
      output_low(MOT_R);
206
      output_low(MOT_R);
175
      return;
207
      return;
176
   };
208
   };
177
   output_low(IRTX);    // Zapni LED na detekci prekazky
209
   output_low(IRTX);    // Zapni LED na detekci prekazky
178
   delay_ms(10);
210
   delay_ms(10);
179
   if (input(IRRX))     // stale nas signal?
211
   if (!IRRX)     // stale nas signal?
180
   {
212
   {
181
      output_low(MOT_L); // neni odraz -> vpred
213
      output_low(MOT_L); // neni odraz -> vpred
182
      output_low(MOT_R);
214
      output_low(MOT_R);
183
      return;
215
      return;
184
   };
216
   };
185
 
217
 
-
 
218
   timer_pom=get_timer1();       // Obsah Timeru1 se musi nacist atomicky
-
 
219
   bb_l[log]=make8(timer_pom,0); // Zaznam do logu
-
 
220
   bb_h[log]=make8(timer_pom,1)|0x80;  // Cihla ma nastaven nejvyssi bit
-
 
221
   if(log<MAXLOG) log++;        // Ukazatel na dalsi zaznam
-
 
222
//!!! Vyzkouset, ze nepreteka LOG
-
 
223
 
186
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
224
//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
187
//   if(stav==cihla) return; // Po druhe nic neobjizdej
225
//   if(stav==cihla) return; // Po druhe nic neobjizdej
188
// Pozor na rozjezd
226
// Pozor na rozjezd
189
 
227
 
190
   if(stav==jizda)   // Objed cihlu
228
   if(stav==jizda)   // Objed cihlu
191
   {
229
   {
192
      #include ".\objizdka_L.c"
230
      #include ".\objizdka_L.c"
193
   }
231
   };
-
 
232
   last_log_odo=get_timer1()+16;  // Pul metru po cihle nezaznamenavej do LOGu
194
}
233
}
195
 
234
 
196
 
235
 
197
//---------------------------------- MAIN --------------------------------------
236
//---------------------------------- MAIN --------------------------------------
198
void main()
237
void main()
199
{
238
{
200
   int8 offset;   // Promena pro ulozeni offsetu
239
   int8 offset;   // Promena pro ulozeni offsetu
201
   int8 r1;       // Rychlost motoru 1
240
   int8 r1;       // Rychlost motoru 1
202
   int8 r2;       // Rychlost motoru 2
241
   int8 r2;       // Rychlost motoru 2
203
 
242
 
204
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
243
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
205
   setup_adc(ADC_CLOCK_INTERNAL);
244
   setup_adc(ADC_CLOCK_INTERNAL);
206
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
245
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
207
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
246
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
208
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
247
   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
209
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
248
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
210
   setup_ccp2(CCP_PWM); // RC2
249
   setup_ccp2(CCP_PWM); // RC2
211
   setup_comparator(NC_NC_NC_NC);
250
   setup_comparator(NC_NC_NC_NC);
212
   setup_vref(FALSE);
251
   setup_vref(FALSE);
213
 
252
 
214
   set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se to nedela automaticky
253
   set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se to nedela automaticky
215
 
254
 
216
   set_pwm1_duty(0);    // Zastav motory
255
   set_pwm1_duty(0);    // Zastav motory
217
   set_pwm2_duty(0);
256
   set_pwm2_duty(0);
218
   output_low(MOT_L);   // Nastav smer vpred
257
   output_low(MOT_L);   // Nastav smer vpred
219
   output_low(MOT_R);
258
   output_low(MOT_R);
220
 
259
 
221
   output_low(IRTX);   // Zapni LED na detekci prekazky
260
   output_low(IRTX);   // Zapni LED na detekci prekazky
222
 
261
 
223
   NightRider(1);    // Aby se poznalo, ze byl RESET
262
   NightRider(1);    // Aby se poznalo, ze byl RESET
224
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
263
                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
225
 
264
 
226
   //... Nastaveni sonaru ...
265
   //... Nastaveni sonaru ...
227
   i2c_start();
266
   i2c_start();
228
   i2c_write(SONAR_adr);
267
   i2c_write(SONAR_ADR);
229
   i2c_write(0x02);  // dosah
268
   i2c_write(0x02);  // dosah
230
   i2c_write(0x03);  // n*43mm
269
   i2c_write(0x03);  // n*43mm
231
   i2c_stop();
270
   i2c_stop();
232
   i2c_start();
271
   i2c_start();
233
   i2c_write(SONAR_adr);
272
   i2c_write(SONAR_ADR);
234
   i2c_write(0x01);  // zesileni
273
   i2c_write(0x01);  // zesileni
235
   i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
274
   i2c_write(0x01);  // male, pro eliminaci echa z minuleho mereni
236
   i2c_stop();
275
   i2c_stop();
237
 
276
 
238
// pro ladeni sonaru
277
// pro ladeni sonaru
239
/*
278
/*
240
   while(true)
279
   while(true)
241
   {
280
   {
242
         i2c_start();     // Sonar Ping
281
         i2c_start();     // Sonar Ping
243
      i2c_write(0xE0);
282
      i2c_write(0xE0);
244
      i2c_write(0x0);
283
      i2c_write(0x0);
245
      i2c_write(0x51);  // 50 mereni v palcich, 51 mereni v cm, 52 v us
284
      i2c_write(0x51);  // 50 mereni v palcich, 51 mereni v cm, 52 v us
246
      i2c_stop();
285
      i2c_stop();
247
      delay_ms(100);
286
      delay_ms(100);
248
      i2c_start();     // Odraz ze sonaru
287
      i2c_start();     // Odraz ze sonaru
249
      i2c_write(0xE0);
288
      i2c_write(0xE0);
250
      i2c_write(0x3);
289
      i2c_write(0x3);
251
      i2c_stop();
290
      i2c_stop();
252
      i2c_start();
291
      i2c_start();
253
      i2c_write(0xE1);
292
      i2c_write(0xE1);
254
      cas=i2c_read(0);
293
      cas=i2c_read(0);
255
      i2c_stop();
294
      i2c_stop();
256
      disp(cas);
295
      disp(cas);
257
   }
296
   }
258
*/
297
*/
259
 
298
 
260
   //... Nastaveni kamery ...
299
   //... Nastaveni kamery ...
261
   i2c_start();      // Soft RESET kamery
300
   i2c_start();      // Soft RESET kamery
262
   i2c_write(CAMERA_adr);        // Pro single slave musi mit vsechny zapisy adresu C0h
301
   i2c_write(CAMERA_ADR);        // Pro single slave musi mit vsechny zapisy adresu C0h
263
   i2c_write(0x12);        // Adresa registru COMH
302
   i2c_write(0x12);        // Adresa registru COMH
264
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
303
   i2c_write(0x80 | 0x24); // Zapis ridiciho slova
265
   i2c_stop();
304
   i2c_stop();
266
 
305
 
267
   i2c_start();      // BW
306
   i2c_start();      // BW
268
   i2c_write(CAMERA_adr);
307
   i2c_write(CAMERA_ADR);
269
   i2c_write(0x28);
308
   i2c_write(0x28);
270
   i2c_write(0b01000001);
309
   i2c_write(0b01000001);
271
   i2c_stop();
310
   i2c_stop();
272
 
311
 
273
/*
312
/*
274
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
313
   i2c_start();      // Contrast (nema podstatny vliv na obraz)
275
   i2c_write(0xC0);
314
   i2c_write(0xC0);
276
   i2c_write(0x05);
315
   i2c_write(0x05);
277
   i2c_write(0xA0);  // 48h
316
   i2c_write(0xA0);  // 48h
278
   i2c_stop();
317
   i2c_stop();
279
 
318
 
280
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
319
   i2c_start();      // Band Filter (pokud by byl problem se zarivkama 50Hz)
281
   i2c_write(0xC0);
320
   i2c_write(0xC0);
282
   i2c_write(0x2D);
321
   i2c_write(0x2D);
283
   i2c_write(0x04 | 0x03);
322
   i2c_write(0x04 | 0x03);
284
   i2c_stop();
323
   i2c_stop();
285
*/
324
*/
286
 
325
 
287
   i2c_start();      // Fame Rate
326
   i2c_start();      // Fame Rate
288
   i2c_write(CAMERA_adr);
327
   i2c_write(CAMERA_ADR);
289
   i2c_write(0x2B);
328
   i2c_write(0x2B);
290
   i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
329
   i2c_write(0x00);  // cca 17ms (puvodni hodnota 5Eh = 20ms)
291
   i2c_stop();
330
   i2c_stop();
292
 
331
 
293
   i2c_start();      // VSTRT
332
   i2c_start();      // VSTRT
294
   i2c_write(CAMERA_adr);
333
   i2c_write(CAMERA_ADR);
295
   i2c_write(0x19);
334
   i2c_write(0x19);
296
   i2c_write(118);   // prostredni radka
335
   i2c_write(118);   // prostredni radka
297
   i2c_stop();
336
   i2c_stop();
298
 
337
 
299
   i2c_start();      // VEND
338
   i2c_start();      // VEND
300
   i2c_write(CAMERA_adr);
339
   i2c_write(CAMERA_ADR);
301
   i2c_write(0x1A);
340
   i2c_write(0x1A);
302
   i2c_write(118);
341
   i2c_write(118);
303
   i2c_stop();
342
   i2c_stop();
304
 
343
 
305
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
344
   NightRider(1);    // Musi se dat cas kamere na AGC a AEC
306
 
345
 
307
   { // Mereni expozice
346
   { // Mereni expozice
308
      int8 t1,t2;
347
      int8 t1,t2;
309
 
348
 
310
      i2c_start();      // Brightness, zacni od uplne tmy
349
      i2c_start();      // Brightness, zacni od uplne tmy
311
      i2c_write(CAMERA_adr);
350
      i2c_write(CAMERA_ADR);
312
      i2c_write(0x06);
351
      i2c_write(0x06);
313
      i2c_write(0);  // 80h default
352
      i2c_write(0);  // 80h default
314
      i2c_stop();
353
      i2c_stop();
315
      delay_ms(50);
354
      delay_ms(50);
316
 
355
 
317
      for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
356
      for(offset=0x04;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 10h
318
      {
357
      {
319
         i2c_start();      // Brightness
358
         i2c_start();      // Brightness
320
         i2c_write(CAMERA_adr);
359
         i2c_write(CAMERA_ADR);
321
         i2c_write(0x06);
360
         i2c_write(0x06);
322
         i2c_write(offset);  // 80h default
361
         i2c_write(offset);  // 80h default
323
         i2c_stop();
362
         i2c_stop();
324
         disp(offset);
363
         disp(offset);
325
         delay_ms(50);
364
         delay_ms(50);
326
 
365
 
327
         t1=0;
366
         t1=0;
328
         t2=0;
367
         t2=0;
329
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
368
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
330
         delay_ms(5);
369
         delay_ms(5);
331
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
370
         while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
332
         set_timer0(0);          // Vynuluj pocitadlo casu
371
         set_timer0(0);          // Vynuluj pocitadlo casu
333
         if(!input(PIX)) continue;
372
         if(!input(PIX)) continue;
334
         while(input(PIX));
373
         while(input(PIX));
335
         t1=get_timer0();    // Precti cas z citace casu hrany
374
         t1=get_timer0();    // Precti cas z citace casu hrany
336
         set_timer0(0);          // Vynuluj pocitadlo casu
375
         set_timer0(0);          // Vynuluj pocitadlo casu
337
         while(!input(PIX));
376
         while(!input(PIX));
338
         t2=get_timer0();
377
         t2=get_timer0();
339
 
378
 
340
         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
379
         if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas?
341
 
380
 
342
         delay_ms(2);   // Preskoc druhou radku z kamery
381
         delay_ms(2);   // Preskoc druhou radku z kamery
343
      };
382
      };
344
      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
383
      delay_ms(1000);   // Nech chvili na displayi zmerenou hodnotu
345
   }
384
   }
346
 
385
 
347
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
386
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
348
   delay_ms(1);
387
   delay_ms(1);
349
   offset=read_adc();
388
   offset=read_adc();
350
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
389
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
351
//   offset += 0x70;         // Jas nebude nikdy nizsi
390
//   offset += 0x70;         // Jas nebude nikdy nizsi
352
   disp(offset);
391
   disp(offset);
353
   i2c_start();            // Brightness
392
   i2c_start();            // Brightness
354
   i2c_write(CAMERA_adr);
393
   i2c_write(CAMERA_adr);
355
   i2c_write(0x06);
394
   i2c_write(0x06);
356
   i2c_write(offset);  // 80h default
395
   i2c_write(offset);  // 80h default
357
   i2c_stop();
396
   i2c_stop();
358
   delay_ms(1000);   // Nech hodnotu chvili na displayi
397
   delay_ms(1000);   // Nech hodnotu chvili na displayi
359
 
398
 
360
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
399
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
361
   delay_ms(1);
400
   delay_ms(1);
362
   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
401
   rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
363
   rrold=rr;
402
   rrold=rr;
364
 
403
 
365
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
404
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
366
                        // a aby se to rozjelo jeste dneska
405
                        // a aby se to rozjelo jeste dneska
367
   stav=start;          // Jsme na startu
406
   stav=start;          // Jsme na startu
368
   set_timer1(0);       // Vynuluj citac odometrie
407
   set_timer1(0);       // Vynuluj citac odometrie
-
 
408
   log=0;               // Zacatek logu v cerne skrince
-
 
409
   last_log_odo=0;      // Posledni zaznam odometrie do logu
-
 
410
 
-
 
411
   if(BUMPER)  // Kdyz nekdo na zacatku drzi naraznik, vymaz log
-
 
412
   {
-
 
413
      bb_l[0]=0;        // Zapis na pozici 0 vzdalenost 0
-
 
414
      bb_h[0]=0;
-
 
415
      SaveLog(0);       // Zapis do EEPROM pocet zaznamu 0, tedy jeden zaznam
-
 
416
      disp(0x55);       // Blikni pro potvrzeni
-
 
417
      delay_ms(200);
-
 
418
      disp(0xAA);
-
 
419
      delay_ms(200);
-
 
420
   }
-
 
421
 
-
 
422
   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
-
 
423
   bb=MAKE16(read_eeprom(1) & 0x7F,read_eeprom(0)); // Precti prvni zaznam
369
 
424
 
370
   // ........................... Hlavni smycka ................................
425
   // ........................... Hlavni smycka ................................
371
   while(true)
426
   while(true)
372
   {
427
   {
373
      int8 pom;
428
      int8 pom;
374
      int8 n;
429
      int8 n;
375
 
430
 
376
next_snap:
431
next_snap:
377
 
432
 
378
      pom=0;
433
      pom=0;
379
      disable_interrupts(GLOBAL);  //----------------------- Critical
434
      disable_interrupts(GLOBAL);  //----------------------- Critical
380
      while(input(HREF));     // Preskoc 1. radku
435
      while(input(HREF));     // Preskoc 1. radku
381
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
436
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
382
      set_timer0(0);          // Vynuluj pocitadlo casu
437
      set_timer0(0);          // Vynuluj pocitadlo casu
383
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
438
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
384
      {
439
      {
385
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
440
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
386
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
441
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
387
         if(!input(PIX))
442
         if(!input(PIX))
388
//         if(!input(PIX))
443
//         if(!input(PIX))
389
         {
444
         {
390
            pom=get_timer0();    // Precti cas z citace casu hrany
445
            pom=get_timer0();    // Precti cas z citace casu hrany
391
            break;
446
            break;
392
         };
447
         };
393
      };
448
      };
394
      while(input(HREF));      // Pockej na shozeni signalu HREF
449
      while(input(HREF));      // Pockej na shozeni signalu HREF
395
 
450
 
396
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
451
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
397
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
452
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
398
      // a vubec to nejak na kraji nefunguje.
453
      // a vubec to nejak na kraji nefunguje.
399
 
454
 
400
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
455
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
401
      delay_us(1000);
456
      delay_us(1000);
402
      delay_us(stred);
457
      delay_us(stred);
403
      delay_us(stred);
458
      delay_us(stred);
404
      delay_us(stred);
459
      delay_us(stred);
405
      delay_us(cas);
460
      delay_us(cas);
406
      delay_us(cas);
461
      delay_us(cas);
407
      output_low(SERVO);
462
      output_low(SERVO);
408
 
463
 
-
 
464
      if(pom==0) // Kamera nevidi caru, poznamenej to do logu
-
 
465
      {
-
 
466
         if(last_log_odo>get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
-
 
467
         {
-
 
468
            timer_pom=get_timer1();          // Timer se musi poznamenat atomicky
-
 
469
            bb_l[log]=make8(timer_pom,0); // Zaznam
-
 
470
            bb_h[log]=make8(timer_pom,1);
-
 
471
            last_log_odo=timer_pom+8;     // Dalsi mereni nejdrive po ujeti 24 cm
-
 
472
            if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam
-
 
473
         }
-
 
474
      }
-
 
475
 
409
      // Elektronicky diferencial
476
      // Elektronicky diferencial
410
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
477
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
411
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
478
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
412
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
479
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
413
 
480
 
414
      enable_interrupts(GLOBAL);    //----------------------- Critical
481
      enable_interrupts(GLOBAL);    //----------------------- Critical
415
 
482
 
416
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
483
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
417
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
484
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
418
 
485
 
419
//      r1<<=1;     // Rychlost je dvojnasobna
486
//      r1<<=1;     // Rychlost je dvojnasobna
420
//      r2<<=1;     // Rozsah 2 az 184
487
//      r2<<=1;     // Rozsah 2 az 184
421
 
488
 
422
/* Nerozumna rychlost po cihle
489
/* Nerozumna rychlost po cihle
423
      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
490
      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
424
      {
491
      {
425
         rr=rrold;
492
         rr=rrold;
426
         stav=pocihle;
493
         stav=pocihle;
427
      };
494
      };
428
*/
495
*/
429
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
496
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
430
      {
497
      {
431
         set_pwm1_duty(r1);
498
         set_pwm1_duty(r1);
432
         set_pwm2_duty(r2);
499
         set_pwm2_duty(r2);
433
      }
500
      }
434
      else
501
      else
435
      {
502
      {
436
         set_pwm1_duty(0);       // Zastaveni
503
         set_pwm1_duty(0);       // Zastaveni
437
         set_pwm2_duty(0);
504
         set_pwm2_duty(0);
438
      };
505
      };
439
 
506
 
440
      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
507
      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
441
      {
508
      {
442
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
509
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
443
         INT0IF=0;                     // Zruseni predesle udalosti od startera
510
         INT0IF=0;                     // Zruseni predesle udalosti od startera
444
         enable_interrupts(INT_EXT);
511
         enable_interrupts(INT_EXT);
445
         enable_interrupts(GLOBAL);
512
         enable_interrupts(GLOBAL);
446
         stav=jizda;
513
         stav=jizda;
447
      };
514
      };
448
 
515
 
449
      if(stav==start)         // Snimkuje, toci servem a ceka na start
516
      if(stav==start)         // Snimkuje, toci servem a ceka na start
450
      {
517
      {
451
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
518
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
452
         Delay_ms(1);
519
         Delay_ms(1);
453
         stred=read_adc();
520
         stred=read_adc();
454
         if(!input(PROXIMITY))
521
         if(!input(PROXIMITY))
455
         {
522
         {
456
            disp(0x80);
523
            disp(0x80);
457
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
524
            while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
458
            set_timer1(0);         // Vynuluj citac odometrie
525
            set_timer1(0);         // Vynuluj citac odometrie
459
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
526
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
460
            set_pwm2_duty(255);
527
            set_pwm2_duty(255);
461
            disp(0x01);
528
            disp(0x01);
462
            while(get_timer1()<=4) // Ujed alespon 12cm
529
            while(get_timer1()<=4) // Ujed alespon 12cm
463
            {
530
            {
464
               set_adc_channel(LMAX);    // Levy UV sensor
531
               set_adc_channel(LMAX);    // Levy UV sensor
465
               delay_us(40);
532
               delay_us(40);
466
               if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
533
               if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
467
               set_adc_channel(RMAX);    // Pravy UV sensor
534
               set_adc_channel(RMAX);    // Pravy UV sensor
468
               delay_us(40);
535
               delay_us(40);
469
               if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
536
               if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
470
               cas=CASAVR-CASMIN;    // Cara je rovne
537
               cas=CASAVR-CASMIN;    // Cara je rovne
471
            };
538
            };
472
            stav=rozjezd;
539
            stav=rozjezd;
473
         };
540
         };
474
      }
541
      }
475
 
542
 
476
      pom=0x80;    // Zobrazeni pozice cary na displayi
543
      pom=0x80;    // Zobrazeni pozice cary na displayi
477
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
544
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
478
      disp(pom);
545
      disp(pom);
479
 
546
 
480
      while(true) // Ve zbytku casu snimku cti krajni UV senzory
547
      while(true) // Ve zbytku casu snimku cti krajni UV senzory a naraznik
481
      {
548
      {
-
 
549
         if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
-
 
550
         {
-
 
551
            set_pwm1_duty(0);    // couvni, rovne dozadu
-
 
552
            set_pwm2_duty(0);
-
 
553
            output_high(MOT_L);
-
 
554
            output_high(MOT_R);
-
 
555
            disp(0x5A);
-
 
556
            SetServo(CASAVR-CASMIN);
-
 
557
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
-
 
558
            output_low(MOT_L);   // Vypni motory
-
 
559
            output_low(MOT_R);
-
 
560
         };
482
         set_adc_channel(LMAX);    // Levy UV sensor
561
         set_adc_channel(LMAX);    // Levy UV sensor
483
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
562
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
484
         if(read_adc()<THR) cas=CASMIN;
563
         if(read_adc()<THR) cas=CASMIN;
485
         set_adc_channel(RMAX);    // Pravy UV sensor
564
         set_adc_channel(RMAX);    // Pravy UV sensor
486
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
565
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
487
         if(read_adc()<THR) cas=CASMAX;
566
         if(read_adc()<THR) cas=CASMAX;
488
      };
567
      };
489
   }
568
   }
490
}
569
}