Subversion Repositories svnkaklik

Rev

Rev 189 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log

Rev 189 Rev 190
Line 1... Line 1...
1 
//******** Mrakomer ******************************************
 1 
//******** Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 189 2007-03-16 16:01:25Z kakl $"
 2 
//"$Id: camerus.c 190 2007-03-16 21:34:07Z 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 
 
Line 17... Line 17...
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
- 
 
 23 
#define MOT_2  PIN_C2      //
22 
#define DATA   PIN_B2      // K modulu LEDbar data
 24 
#define DATA   PIN_B2      // K modulu LEDbar data
23 
#define CP     PIN_B1      // K modulu LEDbar hodiny
 25 
#define CP     PIN_B1      // K modulu LEDbar hodiny
24 
#define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
 26 
#define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
25 
#define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
 27 
#define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
26 
#define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
 28 
#define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
Line 41... Line 43...
41 
 
 43 
 
42 
enum stavy {start,rozjezd,jizda,cihla,cil};
 44 
enum stavy {start,rozjezd,jizda,cihla,cil};
43 
stavy stav;    // Kde jsme na trati
 45 
stavy stav;    // Kde jsme na trati
44 
int8 cas;      // Cas hrany bila/cerna v radce
 46 
int8 cas;      // Cas hrany bila/cerna v radce
45 
 
 47 
 
- 
 
 48 
 
- 
 
 49 
inline void brzda()
- 
 
 50 
{
- 
 
 51 
   int8 n,i;
- 
 
 52 
 
- 
 
 53 
   set_pwm1_duty(0);    // vypni PWM
- 
 
 54 
   set_pwm2_duty(0);
- 
 
 55 
   setup_ccp1(CCP_OFF);
- 
 
 56 
   setup_ccp2(CCP_OFF);
- 
 
 57 
   for (n=0;n<255;n++)
- 
 
 58 
   {
- 
 
 59 
      output_low(MOT_L);
- 
 
 60 
      output_low(MOT_R);
- 
 
 61 
      output_high(MOT_1);
- 
 
 62 
      output_high(MOT_2);
- 
 
 63 
      delay_us(800);
- 
 
 64 
      output_high(MOT_L);
- 
 
 65 
      output_high(MOT_R);
- 
 
 66 
      output_low(MOT_1);
- 
 
 67 
      output_low(MOT_2);
- 
 
 68 
      delay_us(800);
- 
 
 69 
   }
- 
 
 70 
   output_low(MOT_L); // smer vpred
- 
 
 71 
   output_low(MOT_R);
- 
 
 72 
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
- 
 
 73 
   setup_ccp2(CCP_PWM); // RC2
- 
 
 74 
}
- 
 
 75 
 
46 
void SetServo(int8 angle)
 76 
void SetServo(int8 angle)
47 
{
 77 
{
48 
   int8 n, offset;
 78 
   int8 n, offset;
49 
 
 79 
 
50 
   for(n=0; n<20; n++)
 80 
   for(n=0; n<14; n++)
51 
   {
 81 
   {
52 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
 82 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
53 
      Delay_ms(1);
 83 
      Delay_ms(1);
54 
      offset=read_adc();
 84 
      offset=read_adc();
55 
 
 85 
 
Line 66... Line 96...
66 
}
 96 
}
67 
 
 97 
 
68 
#int_EXT
 98 
#int_EXT
69 
EXT_isr()   // Preruseni od prekazky
 99 
EXT_isr()   // Preruseni od prekazky
70 
{
 100 
{
- 
 
 101 
   set_pwm1_duty(0);    // reverz (zabrzdi)
- 
 
 102 
   set_pwm2_duty(0);
- 
 
 103 
   output_high(MOT_L);  // vzad
- 
 
 104 
   output_high(MOT_R);
71 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
 105 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
72 
   output_high(IRTX);   // Vypni LED na detekci prekazky
 106 
   output_high(IRTX);   // Vypni LED na detekci prekazky
73 
   set_pwm1_duty(0);    // Vypni motory
 - 
 
74 
   set_pwm2_duty(0);
 - 
 
75 
   output_low(MOT_L);
 - 
 
76 
   output_low(MOT_R);
 - 
 
77 
   delay_ms(2);
 107 
   delay_ms(2);
78 
   if (!input(IRRX)) return;
 108 
   if (!input(IRRX))    // stale nas signal?
- 
 
 109 
   {
- 
 
 110 
      output_low(MOT_L); // je odraz -> vpred
- 
 
 111 
      output_low(MOT_R);
- 
 
 112 
      return;
- 
 
 113 
   };
79 
   output_low(IRTX);    // Zapni LED na detekci prekazky
 114 
   output_low(IRTX);    // Zapni LED na detekci prekazky
80 
   delay_ms(10);
 115 
   delay_ms(10);
81 
   if (input(IRRX)) return;
 116 
   if (input(IRRX))     // stale nas signal?
82 
 
 117 
   {
83 
   set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
84 
   set_pwm2_duty(0);
 - 
 
85 
   output_high(MOT_L);
 - 
 
86 
   output_high(MOT_R);
 - 
 
87 
   delay_ms(400);
 - 
 
88 
   output_low(MOT_L); // zastav
 118 
      output_low(MOT_L); // neni odraz -> vpred
89 
   output_low(MOT_R);
 119 
      output_low(MOT_R);
- 
 
 120 
      return;
- 
 
 121 
   };
- 
 
 122 
 
- 
 
 123 
   SetServo((CASAVR-CASMIN));   // rovne
- 
 
 124 
   delay_ms(100);    // Doba reverzace
- 
 
 125 
 
- 
 
 126 
   brzda();
- 
 
 127 
 
90 
   if (stav==cihla) while(true); // Zastav na furt
 128 
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
- 
 
 129 
 
91 
   if(stav==jizda)
 130 
   if(stav==jizda)   // Objed cihlu
92 
   {
 131 
   {
93 
      int n;
 132 
      int n;
94 
/*
 133 
 
95 
      SetServo(CASAVR-CASMIN);
 134 
      SetServo(CASMIN);   // prudce doleva
- 
 
 135 
      delay_ms(200);
- 
 
 136 
 
96 
      set_pwm1_duty(40); // pomalu couvej
 137 
      set_pwm1_duty(150);  // vpred
97 
      set_pwm2_duty(40);
 138 
      set_pwm2_duty(200);
98 
      output_high(MOT_L);
 139 
      output_low(MOT_L);
99 
      output_high(MOT_R);
 140 
      output_low(MOT_R);
100 
      n=0;
 141 
      n=0;
101 
      while(true)
 142 
      while(true)
102 
      {
 143 
      {
103 
         while(input(ODO));
 144 
         while(input(ODO));
104 
         while(!input(ODO));
 145 
         while(!input(ODO));
105 
         n++;
 146 
         n++;
106 
         if(n==6) break;
 147 
         if(n==5) break;
107 
      }
 148 
      }
108 
      set_pwm1_duty(255);    // reverz (zabrzdi)
 149 
      set_pwm1_duty(0);    // reverz (zabrzdi)
109 
      set_pwm2_duty(255);
 - 
 
110 
      output_low(MOT_L);
 - 
 
111 
      output_low(MOT_R);
 - 
 
112 
      delay_ms(100);
 - 
 
113 
      set_pwm1_duty(0);  // Zastav
 - 
 
114 
      set_pwm2_duty(0);
 150 
      set_pwm2_duty(0);
115 
      output_low(MOT_L);
 151 
      output_high(MOT_L);
116 
      output_low(MOT_R);
 152 
      output_high(MOT_R);
117 
*/
 - 
 
118 
      delay_ms(1000);
 - 
 
119 
 
 153 
 
120 
      SetServo((CASAVR-CASMIN)-20);   // doleva
 154 
      SetServo((CASAVR-CASMIN));   // rovne
121 
      set_pwm1_duty(150);  // vpred
 155 
      set_pwm1_duty(140);  // vpred
122 
      set_pwm2_duty(200);
 156 
      set_pwm2_duty(140);
123 
      output_low(MOT_L);
 157 
      output_low(MOT_L);
124 
      output_low(MOT_R);
 158 
      output_low(MOT_R);
125 
      stav=cihla;
 - 
 
126 
      n=0;
 159 
      n=0;
127 
      while(true)
 160 
      while(true)
128 
      {
 161 
      {
129 
         while(input(ODO));
 162 
         while(input(ODO));
130 
         while(!input(ODO));
 163 
         while(!input(ODO));
131 
         n++;
 164 
         n++;
132 
         if(n==10) break;
 165 
         if(n==6) break;
133 
      }
 166 
      }
134 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
135 
      set_pwm2_duty(0);
 - 
 
136 
      output_high(MOT_L);
 - 
 
137 
      output_high(MOT_R);
 - 
 
138 
 
 167 
 
- 
 
 168 
      brzda();
- 
 
 169 
 
139 
      SetServo((CASAVR-CASMIN));   // rovne
 170 
      SetServo((CASAVR-CASMIN)+70);   // doprava
140 
      set_pwm1_duty(140);  // vpred
 171 
      set_pwm1_duty(140);  // vpred
141 
      set_pwm2_duty(140);
 172 
      set_pwm2_duty(140);
142 
      output_low(MOT_L);
 173 
      output_low(MOT_L);
143 
      output_low(MOT_R);
 174 
      output_low(MOT_R);
144 
      stav=cihla;
 - 
 
145 
      n=0;
 175 
      n=0;
146 
      while(true)
 176 
      while(true)
147 
      {
 177 
      {
148 
         while(input(ODO));
 178 
         while(input(ODO));
149 
         while(!input(ODO));
 179 
         while(!input(ODO));
150 
         n++;
 180 
         n++;
151 
         if(n==10) break;
 181 
         if(n==6) break;
152 
      }
 182 
      }
- 
 
 183 
 
153 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 184 
      set_pwm1_duty(0);    // reverz (zabrzdi)
154 
      set_pwm2_duty(0);
 185 
      set_pwm2_duty(0);
155 
      output_high(MOT_L);
 186 
      output_high(MOT_L);
156 
      output_high(MOT_R);
 187 
      output_high(MOT_R);
157 
      delay_ms(200);
 188 
      delay_ms(100);
158 
      set_pwm1_duty(150);  // Zastav
 - 
 
159 
      set_pwm2_duty(150);
 - 
 
160 
      output_high(MOT_L);
 - 
 
161 
      output_high(MOT_R);
 - 
 
- 
 
 189 
 
162 
      delay_ms(1000);
 190 
      brzda();
163 
 
 191 
 
164 
      SetServo(CASMIN);   // max. doleva
 192 
      SetServo(CASMIN);   // max. doleva
165 
      set_pwm1_duty(0);  // vzad
 193 
      set_pwm1_duty(0);   // vzad
166 
      set_pwm2_duty(20);
 194 
      set_pwm2_duty(20);
167 
      output_low(MOT_L);
 195 
      output_low(MOT_L);
168 
      output_high(MOT_R);
 196 
      output_high(MOT_R);
169 
      stav=cihla;
 - 
 
170 
      n=0;
 197 
      n=0;
171 
      while(true)
 198 
      while(true)
172 
      {
 199 
      {
173 
         while(input(ODO));
 200 
         while(input(ODO));
174 
         while(!input(ODO));
 201 
         while(!input(ODO));
175 
         n++;
 202 
         n++;
176 
         if(n==10) break;
 203 
         if(n==5) break;
177 
      }
 204 
      }
178 
      set_pwm1_duty(0);    // reverz (zabrzdi)
 205 
      set_pwm1_duty(0);    // reverz (zabrzdi)
179 
      set_pwm2_duty(255);
 206 
      set_pwm2_duty(255);
180 
      output_low(MOT_L);
 207 
      output_low(MOT_L);
181 
      output_low(MOT_R);
 208 
      output_low(MOT_R);
182 
      delay_ms(100);
 209 
      delay_ms(100);
183 
      set_pwm1_duty(0);  // Zastav
 - 
 
184 
      set_pwm2_duty(0);
 - 
 
185 
      output_low(MOT_L);
 - 
 
186 
      output_low(MOT_R);
 - 
 
187 
      delay_ms(500);
 - 
 
188 
 
 210 
 
- 
 
 211 
      brzda();
189 
 
 212 
 
190 
      SetServo((CASAVR-CASMIN)+5);   // mirne doprava
 213 
      SetServo((CASAVR-CASMIN)+20);   // mirne doprava
191 
      set_pwm1_duty(180);  // vpred
 214 
      set_pwm1_duty(190);  // vpred
192 
      set_pwm2_duty(180);
 215 
      set_pwm2_duty(190);
193 
      output_low(MOT_L);
 216 
      output_low(MOT_L);
194 
      output_low(MOT_R);
 217 
      output_low(MOT_R);
195 
      stav=cihla;
 - 
 
196 
      n=0;
 218 
      n=0;
197 
      while(true)
 219 
      while(true)
198 
      {
 220 
      {
199 
         while(input(ODO));
 221 
         while(input(ODO));
200 
         while(!input(ODO));
 222 
         while(!input(ODO));
201 
         n++;
 223 
         n++;
202 
         if(n==12) break;
 224 
         if(n==14) break;
203 
      }
 225 
      }
- 
 
 226 
/*
204 
      set_pwm1_duty(0);  // Zastav
 227 
      set_pwm1_duty(0);    // reverz (zabrzdi)
205 
      set_pwm2_duty(0);
 228 
      set_pwm2_duty(0);
206 
      output_low(MOT_L);
 229 
      output_high(MOT_L);
207 
      output_low(MOT_R);
 230 
      output_high(MOT_R);
208 
      delay_ms(500);
 231 
      delay_ms(100);
209 
 
 232 
*/
- 
 
 233 
      brzda();
210 
 
 234
211 
      cas=CASMIN;  // Cara je vlevo
 235 
      cas=CASMIN;  // Cara je vlevo
212 
 
 236 
 
213 
      stav=cihla;
 237 
      stav=cihla;
214 
   }
 238 
   }
Line 342... Line 366...
342 
      i2c_write(0x06);
 366 
      i2c_write(0x06);
343 
      i2c_write(0);  // 80h default
 367 
      i2c_write(0);  // 80h default
344 
      i2c_stop();
 368 
      i2c_stop();
345 
      delay_ms(50);
 369 
      delay_ms(50);
346 
 
 370 
 
347 
      for(offset=0x60;offset<=254;offset+=0x04)   // Zacni od jasu 60h
 371 
      for(offset=0x60;offset<(255-0x04);offset+=0x04)   // Zacni od jasu 60h
348 
      {
 372 
      {
349 
         i2c_start();      // Brightness
 373 
         i2c_start();      // Brightness
350 
         i2c_write(0xC0);
 374 
         i2c_write(0xC0);
351 
         i2c_write(0x06);
 375 
         i2c_write(0x06);
352 
         i2c_write(offset);  // 80h default
 376 
         i2c_write(offset);  // 80h default
Line 376... Line 400...
376 
 
 400 
 
377 
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
 401 
   set_adc_channel(CERVENA);   // --- Kroutitko pro jas ---
378 
   delay_ms(1);
 402 
   delay_ms(1);
379 
   offset=read_adc()>>1;   // 0-127
 403 
   offset=read_adc()>>1;   // 0-127
380 
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
 404 
   offset &= 0b11111100;   // Dva nejnizsi bity ignoruj
- 
 
 405 
   offset += 0x70;         // Jas nebude nikdy nizsi
381 
   disp(offset);
 406 
   disp(offset);
382 
   i2c_start();            // Brightness
 407 
   i2c_start();            // Brightness
383 
   i2c_write(0xC0);
 408 
   i2c_write(0xC0);
384 
   i2c_write(0x06);
 409 
   i2c_write(0x06);
385 
   i2c_write(offset+0x70);  // 80h default
 410 
   i2c_write(offset);  // 80h default
386 
   i2c_stop();
 411 
   i2c_stop();
387 
   delay_ms(1000);   // Nech hodnotu chvili na displayi
 412 
   delay_ms(1000);   // Nech hodnotu chvili na displayi
388 
 
 413 
 
389 
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
 414 
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
390 
   delay_ms(1);
 415 
   delay_ms(1);
Line 473... Line 498...
473 
         disp(0x80);
 498 
         disp(0x80);
474 
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
 499 
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
475 
         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
 500 
         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
476 
         set_pwm2_duty(255);
 501 
         set_pwm2_duty(255);
477 
         disp(0x1);
 502 
         disp(0x1);
478 
         delay_ms(200);
 503 
         delay_ms(400);
479 
         stav=jizda;
 504 
         stav=jizda;
480 
      }
 505 
      }
481 
 
 506 
 
482 
      pom=0x80;    // Zobrazeni pozice cary na displayi
 507 
      pom=0x80;    // Zobrazeni pozice cary na displayi
483 
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
 508 
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;