Subversion Repositories svnkaklik

Rev

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

Rev 198 Rev 200
Line 1... Line 1...
1 
//******** Robot Camerus pro IstRobot 2007 ************
 1 
//******** Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 198 2007-03-18 10:13:14Z kakl $"
 2 
//"$Id: camerus.c 200 2007-03-18 20:54:40Z 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  LMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
 10 
#define  RMAX     4  // AN4/RA5 - leve cidlo na vyjeti z cary
11 
#define  RMAX     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
Line 21... Line 21...
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
 26 
//#define ODO       PIN_A4      // Ze snimace z odometrie z praveho kola / nove je to na RC0 na TIMER1
27 
#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
 27 
#define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
28 
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
 28 
#define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
29 
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
 29 
#define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
30 
 
 30 
 
31 
#define CASMIN 6           // Rozsah radku snimace
 31 
#define CASMIN 6           // Rozsah radku snimace
Line 46... Line 46...
46 
stavy stav;       // Kde jsme na trati
 46 
stavy stav;       // Kde jsme na trati
47 
int8 cas;         // Cas hrany bila/cerna v radce
 47 
int8 cas;         // Cas hrany bila/cerna v radce
48 
int8 stred;       // Vystredeni kolecka
 48 
int8 stred;       // Vystredeni kolecka
49 
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
 49 
int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
50 
 
 50 
 
- 
 
 51 
// Zobrazeni jednoho byte na modulu LEDbar
- 
 
 52 
void disp(int8 x)
- 
 
 53 
{
- 
 
 54 
   int n;
- 
 
 55 
 
- 
 
 56 
   x=~x;
- 
 
 57 
   for(n=0;n<=7;n++)
- 
 
 58 
   {
- 
 
 59 
      if (x & 1 == 1) output_high(DATA); else output_low(DATA);
- 
 
 60 
      output_high(CP);
- 
 
 61 
      x>>=1;
- 
 
 62 
      output_low(CP);
- 
 
 63 
   }
- 
 
 64 
}
- 
 
 65 
 
- 
 
 66 
// Blikani LEDbarem ve stilu Night Rider
- 
 
 67 
void NightRider(int8 x)
- 
 
 68 
{
- 
 
 69 
   int n,i,j;
- 
 
 70 
 
- 
 
 71 
   for(j=0;j<x;j++)
- 
 
 72 
   {
- 
 
 73 
      i=0x01;
- 
 
 74 
      for(n=0;n<7;n++)
- 
 
 75 
      {
- 
 
 76 
         disp(i);
- 
 
 77 
         rotate_left(&i, 1);
- 
 
 78 
         delay_ms(40);
- 
 
 79 
      }
- 
 
 80 
      for(n=0;n<7;n++)
- 
 
 81 
      {
- 
 
 82 
         disp(i);
- 
 
 83 
         rotate_right(&i, 1);
- 
 
 84 
         delay_ms(40);
- 
 
 85 
      }
- 
 
 86 
   }
- 
 
 87 
   disp(i);
- 
 
 88 
   delay_ms(40);
- 
 
 89 
   i=0;
- 
 
 90 
   disp(i);
- 
 
 91 
}
- 
 
 92 
 
- 
 
 93 
// Brzdeni motorama stridou 1:1
51 
inline void brzda()
 94 
void brzda()
52 
{
 95 
{
53 
   int8 n,i;
 96 
   int8 n,i;
54 
 
 97 
 
55 
   set_pwm1_duty(0);    // vypni PWM
 98 
   set_pwm1_duty(0);    // vypni PWM
56 
   set_pwm2_duty(0);
 99 
   set_pwm2_duty(0);
Line 124... Line 167...
124 
   set_pwm2_duty(140);
 167 
   set_pwm2_duty(140);
125 
   output_low(MOT_L);
 168 
   output_low(MOT_L);
126 
   output_low(MOT_R);
 169 
   output_low(MOT_R);
127 
 
 170 
 
128 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 171 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 172 
//!!! while(true) {disp(MAKE8(get_timer1(),0)); delay_ms(10);};
- 
 
 173 
 
129 
   while(true)
 174 
   while(true)
130 
   {
 175 
   {
131 
      if(!input(PROXIMITY)) break; // Je cihla blizko?
 176 
      if(!input(PROXIMITY)) break; // Je cihla blizko?
132 
      if(get_timer1()>=odocounter+7) return; // nedojeli jsme k cihle, jed dal
 177 
      if(get_timer1()>(odocounter+7)) return; // nedojeli jsme k cihle, jed dal
133 
   };
 178 
   };
134 
   set_pwm1_duty(0);    // reverz (zabrzdi)
 179 
   set_pwm1_duty(0);    // reverz (zabrzdi)
135 
   set_pwm2_duty(0);
 180 
   set_pwm2_duty(0);
136 
   output_high(MOT_L);
 181 
   output_high(MOT_L);
137 
   output_high(MOT_R);
 182 
   output_high(MOT_R);
Line 144... Line 189...
144 
   {
 189 
   {
145 
      #include ".\objizdka_R.c"
 190 
      #include ".\objizdka_R.c"
146 
   }
 191 
   }
147 
}
 192 
}
148 
 
 193 
 
149 
// Zobrazeni jednoho byte na modulu LEDbar
 - 
 
150 
void disp(int8 x)
 - 
 
151 
{
 - 
 
152 
   int n;
 - 
 
153 
 
 - 
 
154 
   x=~x;
 - 
 
155 
   for(n=0;n<=7;n++)
 - 
 
156 
   {
 - 
 
157 
      if (x & 1 == 1) output_high(DATA); else output_low(DATA);
 - 
 
158 
      output_high(CP);
 - 
 
159 
      x>>=1;
 - 
 
160 
      output_low(CP);
 - 
 
161 
   }
 - 
 
162 
}
 - 
 
163 
 
 - 
 
164 
// Blikani LEDbarem ve stilu Night Rider
 - 
 
165 
void NightRider(int8 x)
 - 
 
166 
{
 - 
 
167 
   int n,i,j;
 - 
 
168 
 
 - 
 
169 
   for(j=0;j<x;j++)
 - 
 
170 
   {
 - 
 
171 
      i=0x01;
 - 
 
172 
      for(n=0;n<7;n++)
 - 
 
173 
      {
 - 
 
174 
         disp(i);
 - 
 
175 
         rotate_left(&i, 1);
 - 
 
176 
         delay_ms(40);
 - 
 
177 
      }
 - 
 
178 
      for(n=0;n<7;n++)
 - 
 
179 
      {
 - 
 
180 
         disp(i);
 - 
 
181 
         rotate_right(&i, 1);
 - 
 
182 
         delay_ms(40);
 - 
 
183 
      }
 - 
 
184 
   }
 - 
 
185 
   disp(i);
 - 
 
186 
   delay_ms(40);
 - 
 
187 
   i=0;
 - 
 
188 
   disp(i);
 - 
 
189 
}
 - 
 
190 
 
 - 
 
191 
 
 194 
 
192 
//---------------------------------- MAIN --------------------------------------
 195 
//---------------------------------- MAIN --------------------------------------
193 
void main()
 196 
void main()
194 
{
 197 
{
195 
   int8 offset;   // Promena pro ulozeni ovsetu
 198 
   int8 offset;   // Promena pro ulozeni ovsetu
Line 325... Line 328...
325 
   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 !!!
326 
 
 329 
 
327 
   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
 330 
   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
328 
   stav=start;
 331 
   stav=start;
329 
   trasa=0;
 332 
   trasa=0;
- 
 
 333 
   set_timer1(0);         // Vynuluj citac odometrie
330 
 
 334 
 
331 
   // ......... Hlavni smycka .........
 335 
   // ........................... Hlavni smycka ................................
332 
   while(true)
 336 
   while(true)
333 
   {
 337 
   {
334 
      int8 pom;
 338 
      int8 pom;
335 
      int8 n;
 339 
      int8 n;
336 
 
 340 
 
- 
 
 341 
next_snap:
- 
 
 342 
 
337 
      pom=0;
 343 
      pom=0;
- 
 
 344 
      while(input(HREF));     // Preskoc 1. radku
338 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
 345 
      while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
339 
      set_timer0(0);          // Vynuluj pocitadlo casu
 346 
      set_timer0(0);          // Vynuluj pocitadlo casu
340 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
 347 
      while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
341 
      {
 348 
      {
342 
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
 349 
// !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ?
343 
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
 350 
         if(!input(PIX))   // Pokud se X-krat za sebou precetla CERNA
Line 384... Line 391...
384 
      {
 391 
      {
385 
         set_pwm1_duty(0);       // Zastaveni
 392 
         set_pwm1_duty(0);       // Zastaveni
386 
         set_pwm2_duty(0);
 393 
         set_pwm2_duty(0);
387 
      }
 394 
      }
388 
 
 395 
 
- 
 
 396 
//!!! trasu zrusit a predelat to na odometrii
389 
      if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
 397 
      if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
390 
      {
 398 
      {
391 
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
 399 
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
392 
         INT0IF=0;                     // Zruseni predesle udalosti od startera
 400 
         INT0IF=0;                     // Zruseni predesle udalosti od startera
393 
         enable_interrupts(INT_EXT);
 401 
         enable_interrupts(INT_EXT);
Line 409... Line 417...
409 
            disp(0x1);
 417 
            disp(0x1);
410 
            delay_ms(300);
 418 
            delay_ms(300);
411 
            stav=jizda;
 419 
            stav=jizda;
412 
         };
 420 
         };
413 
      }
 421 
      }
414 
 
 422
415 
      pom=0x80;    // Zobrazeni pozice cary na displayi
 423 
      pom=0x80;    // Zobrazeni pozice cary na displayi
416 
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
 424 
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
417 
      disp(pom);
 425 
      disp(pom);
- 
 
 426 
 
- 
 
 427 
      while(true)
- 
 
 428 
      {
- 
 
 429 
         set_adc_channel(LMAX);    // Levy UV sensor
- 
 
 430 
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
- 
 
 431 
         if(read_adc()<128) cas=CASMIN;
- 
 
 432 
         set_adc_channel(RMAX);    // Pravy UV sensor
- 
 
 433 
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
- 
 
 434 
         if(read_adc()<128) cas=CASMAX;
- 
 
 435 
      };
418 
   };
 436 
   }
419 
}
 437 
}