WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/camerus.c



//********* Robot Camerus pro IstRobot 2007 ************
//"$Id: camerus.c 252 2007-04-24 08:53:01Z kakl $"
//*****************************************************
 
#include ".\camerus.h"
 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 
// Rychlostni konstanty
#define RR_CIHLA     25       // Rozumna rychlost pro objizdeni cihly
#define RR_PRERUSENI 20       // Rozumna rychlost pro priblizeni se k preruseni
#define BRZDNA_DRAHA 0x20     // Jak daleko pred problemem se zacne brzdit
#define TUHOS        100      // Jak dlouho se bude couvat po narazu na naraznik
#define ODODO_CIHLA       0xFFF
#define ODODO_TUNEL       0xFFF
#define ODODO_PRERUSENI   0xFFF
 
// Adresy IIC periferii

 
// Zaznam do Logu do RAM
void LogLog(int8 flag, int16 gap)
{
   int16 timer_pom;
 
   timer_pom=get_timer1();          // Timer se musi vycist atomicky
   bb_l[log]=make8(timer_pom,0); // Zaznam
   bb_h[log]=make8(timer_pom,1);
   bb_f[log]=flag;   // Typ zaznamu
   if(log<MAXLOG) log++;         // Ukazatel na dalsi zaznam

void ReadBlackBox()
{
   last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?
   {
      int8 n,i;
 
      i=0;
      for(n=0;n<=last_log;n++)
      {
         if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
         if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
         if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1));
      }
   }
}
 
 
// Brzdeni motorama stridou 1:1
void brzda()

   output_high(MOT_R);
   disp(0xA5);
   SetServo(CASAVR-CASMIN);
}
 
#include ".\diag.c"
 
//---------------------------- INT --------------------------------
#int_EXT
EXT_isr()   // Preruseni od prekazky
{

 
   setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
   setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
//!!!   setup_timer_2(T2_DIV_BY_16,255,1);        // Casovac PWM motoru
   setup_timer_2(T2_DIV_BY_4,255,1);        // Casovac PWM motoru
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
   setup_ccp2(CCP_PWM); // RC2
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
 

   i2c_stop();
   delay_ms(1000);   // Nech hodnotu chvili na displayi
 
   set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
   delay_ms(1);
//!!!   rr=read_adc()>>2; // 0-63  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
   rr=read_adc()>>3; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!
   rrold=rr;
 
   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
                        // a aby se to rozjelo jeste dneska
   stav=start;          // Jsme na startu

      if(input(PROXIMITY) && ((stav==jizda)||(stav==cihla))) // Tunel
      {
         if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu
         {
            LogLog(0,16);   // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm
            rr=rrold;      // Vjeli jsme do tunelu, znovu jed rychle
         }
      };
 
//ODODO
      ododo=get_timer1();
      if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;
      if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;
      if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;
 
      // Elektronicky diferencial 2. cast
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);     // rozsah 1 az 92 pro rr=0                                                                   // rozsah 1 az 154 pro rr=63
 
//!!! pro zatuhle prevodovky
      r1<<=1;     // Rychlost je dvojnasobna
      r2<<=1;     // Rozsah 2 az 184 pro rr=0
 
      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle))      // Jizda
      {
         set_pwm1_duty(r1);
         set_pwm2_duty(r2);

      {
         if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!
         {
            bum();
            SaveLog(log-1);      // Zapis Black Boxu do EEPROM
            delay_ms(TUHOS); //!!! Zatuhle prevodovky
            set_pwm1_duty(200);   // pomalu vpred
            set_pwm2_duty(200);
            output_low(MOT_L);
            output_low(MOT_R);
            cas=CASAVR-CASMIN;
         };
         set_adc_channel(LMAX);    // Levy UV sensor
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
         if(read_adc()<THR) cas=CASMIN;
         set_adc_channel(RMAX);    // Pravy UV sensor

Rev 249	Rev 252
Line 1...	Line 1...
1	`//******* Robot Camerus pro IstRobot 2007 **********`	1	`//******* Robot Camerus pro IstRobot 2007 **********`
2	`//"$Id: camerus.c 249 2007-04-23 12:26:15Z kakl $"`	2	`//"$Id: camerus.c 252 2007-04-24 08:53:01Z kakl $"`
3	`//*****************************************************`	3	`//*****************************************************`
4		4
5	`#include ".\camerus.h"`	5	`#include ".\camerus.h"`
6		6
7	`#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery`	7	`#USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery`
8		8
9	`// Rychlostni konstanty`	9	`// Rychlostni konstanty`
10	`#define RR_CIHLA 30 // Rozumna rychlost pro objizdeni cihly`	10	`#define RR_CIHLA 25 // Rozumna rychlost pro objizdeni cihly`
11	`#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni`	11	`#define RR_PRERUSENI 20 // Rozumna rychlost pro priblizeni se k preruseni`
12	`#define BRZDNA_DRAHA 0x20 // Jak daleko pred problemem se zacne brzdit`	12	`#define BRZDNA_DRAHA 0x20 // Jak daleko pred problemem se zacne brzdit`
-		13	`#define TUHOS 100 // Jak dlouho se bude couvat po narazu na naraznik`
13	`#define ODODO_CIHLA 0xFFF`	14	`#define ODODO_CIHLA 0xFFF`
14	`#define ODODO_TUNEL 0xFFF`	15	`#define ODODO_TUNEL 0xFFF`
15	`#define ODODO_PRERUSENI 0xFFF`	16	`#define ODODO_PRERUSENI 0xFFF`
16		17
17	`// Adresy IIC periferii`	18	`// Adresy IIC periferii`
Line 141...	Line 142...
141		142
142	`// Zaznam do Logu do RAM`	143	`// Zaznam do Logu do RAM`
143	`void LogLog(int8 flag, int16 gap)`	144	`void LogLog(int8 flag, int16 gap)`
144	`{`	145	`{`
145	`int16 timer_pom;`	146	`int16 timer_pom;`
146		147
147	`timer_pom=get_timer1(); // Timer se musi vycist atomicky`	148	`timer_pom=get_timer1(); // Timer se musi vycist atomicky`
148	`bb_l[log]=make8(timer_pom,0); // Zaznam`	149	`bb_l[log]=make8(timer_pom,0); // Zaznam`
149	`bb_h[log]=make8(timer_pom,1);`	150	`bb_h[log]=make8(timer_pom,1);`
150	`bb_f[log]=flag; // Typ zaznamu`	151	`bb_f[log]=flag; // Typ zaznamu`
151	`if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam`	152	`if(log<MAXLOG) log++; // Ukazatel na dalsi zaznam`
Line 156...	Line 157...
156	`void ReadBlackBox()`	157	`void ReadBlackBox()`
157	`{`	158	`{`
158	`last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?`	159	`last_log=read_eeprom(EEMAX); // Kolik zaznamu mame od minule poznamenano?`
159	`{`	160	`{`
160	`int8 n,i;`	161	`int8 n,i;`
161		162
162	`i=0;`	163	`i=0;`
163	`for(n=0;n<=last_log;n++)`	164	`for(n=0;n<=last_log;n++)`
164	`{`	165	`{`
165	`if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1));`	166	`if(read_eeprom(i)==0) odo_tunel=MAKE16(read_eeprom(i+2),read_eeprom(i+1));`
166	`if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1));`	167	`if(read_eeprom(i)==0xFF) odo_cihla=MAKE16(read_eeprom(i+2),read_eeprom(i+1));`
167	`if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1));`	168	`if((read_eeprom(i)>0) && (read_eeprom(i)<0xFF)) odo_preruseni=MAKE16(read_eeprom(i+2),read_eeprom(i+1));`
168	`}`	169	`}`
169	`}`	170	`}`
170	`}`	171	`}`
171		172
172		173
173	`// Brzdeni motorama stridou 1:1`	174	`// Brzdeni motorama stridou 1:1`
174	`void brzda()`	175	`void brzda()`
Line 237...	Line 238...
237	`output_high(MOT_R);`	238	`output_high(MOT_R);`
238	`disp(0xA5);`	239	`disp(0xA5);`
239	`SetServo(CASAVR-CASMIN);`	240	`SetServo(CASAVR-CASMIN);`
240	`}`	241	`}`
241		242
242	`#include ".\diag.c"`	243	`#include ".\diag.c"`
243		244
244	`//---------------------------- INT --------------------------------`	245	`//---------------------------- INT --------------------------------`
245	`#int_EXT`	246	`#int_EXT`
246	`EXT_isr() // Preruseni od prekazky`	247	`EXT_isr() // Preruseni od prekazky`
247	`{`	248	`{`
Line 306...	Line 307...
306		307
307	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`	308	`setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek`
308	`setup_adc(ADC_CLOCK_INTERNAL);`	309	`setup_adc(ADC_CLOCK_INTERNAL);`
309	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`	310	`setup_timer_0(RTCC_INTERNAL\|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce`
310	`setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola`	311	`setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola`
-		312	`//!!! setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`
311	`setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru`	313	`setup_timer_2(T2_DIV_BY_4,255,1); // Casovac PWM motoru`
312	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`	314	`setup_ccp1(CCP_PWM); // RC1 // PWM pro motory`
313	`setup_ccp2(CCP_PWM); // RC2`	315	`setup_ccp2(CCP_PWM); // RC2`
314	`setup_comparator(NC_NC_NC_NC);`	316	`setup_comparator(NC_NC_NC_NC);`
315	`setup_vref(FALSE);`	317	`setup_vref(FALSE);`
316		318
Line 445...	Line 447...
445	`i2c_stop();`	447	`i2c_stop();`
446	`delay_ms(1000); // Nech hodnotu chvili na displayi`	448	`delay_ms(1000); // Nech hodnotu chvili na displayi`
447		449
448	`set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---`	450	`set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru ---`
449	`delay_ms(1);`	451	`delay_ms(1);`
-		452	`//!!! rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!`
450	`rr=read_adc()>>2; // 0-63 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!`	453	`rr=read_adc()>>3; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni!`
451	`rrold=rr;`	454	`rrold=rr;`
452		455
453	`cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh`	456	`cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh`
454	`// a aby se to rozjelo jeste dneska`	457	`// a aby se to rozjelo jeste dneska`
455	`stav=start; // Jsme na startu`	458	`stav=start; // Jsme na startu`
Line 534...	Line 537...
534	`if(input(PROXIMITY) && ((stav==jizda)\|\|(stav==cihla))) // Tunel`	537	`if(input(PROXIMITY) && ((stav==jizda)\|\|(stav==cihla))) // Tunel`
535	`{`	538	`{`
536	`if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu`	539	`if(last_log_odo<get_timer1()) // Aby nebyly zaznamy v logu prilis huste, musi se napred neco ujet od minuleho zaznamu`
537	`{`	540	`{`
538	`LogLog(0,16); // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm`	541	`LogLog(0,16); // Priznak tunelu; dalsi mereni nejdrive po ujeti 48 cm`
539	`rr=rrold; // Vjeli jsme do tunelu, znovu jed rychle`	542	`rr=rrold; // Vjeli jsme do tunelu, znovu jed rychle`
540	`}`	543	`}`
541	`};`	544	`};`
542		545
543	`//ODODO`	546	`//ODODO`
544	`ododo=get_timer1();`	547	`ododo=get_timer1();`
545	`if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;`	548	`if((ododo>odo_preruseni)&&(ododo<(odo_preruseni+8))) rr=RR_PRERUSENI;`
546	`if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;`	549	`if((ododo>odo_cihla)&&(ododo<(odo_cihla+8))) rr=RR_PRERUSENI;`
547	`if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;`	550	`if((ododo>odo_tunel)&&(ododo<(odo_tunel+8))) rr=RR_PRERUSENI;`
548		551
549	`// Elektronicky diferencial 2. cast`	552	`// Elektronicky diferencial 2. cast`
550	`if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni`	553	`if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni`
551	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); // rozsah 1 az 92 pro rr=0`	-
552	`// rozsah 1 az 154 pro rr=63`	554	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); // rozsah 1 az 92 pro rr=0 // rozsah 1 az 154 pro rr=63`
553		555
-		556	`//!!! pro zatuhle prevodovky`
554	`// r1<<=1; // Rychlost je dvojnasobna`	557	`r1<<=1; // Rychlost je dvojnasobna`
555	`// r2<<=1; // Rozsah 2 az 184 pro rr=0`	558	`r2<<=1; // Rozsah 2 az 184 pro rr=0`
556		559
557	`if ((stav==jizda)\|\|(stav==cihla)\|\|(stav==rozjezd)) //\|\|(stav==pocihle)) // Jizda`	560	`if ((stav==jizda)\|\|(stav==cihla)\|\|(stav==rozjezd)) //\|\|(stav==pocihle)) // Jizda`
558	`{`	561	`{`
559	`set_pwm1_duty(r1);`	562	`set_pwm1_duty(r1);`
560	`set_pwm2_duty(r2);`	563	`set_pwm2_duty(r2);`
Line 609...	Line 612...
609	`{`	612	`{`
610	`if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!`	613	`if(BUMPER) // Sakra, do neceho jsme narazili a nevideli jsme to!`
611	`{`	614	`{`
612	`bum();`	615	`bum();`
613	`SaveLog(log-1); // Zapis Black Boxu do EEPROM`	616	`SaveLog(log-1); // Zapis Black Boxu do EEPROM`
-		617	`delay_ms(TUHOS); //!!! Zatuhle prevodovky`
614	`set_pwm1_duty(160); // pomalu vpred`	618	`set_pwm1_duty(200); // pomalu vpred`
615	`set_pwm2_duty(160);`	619	`set_pwm2_duty(200);`
616	`output_low(MOT_L);`	620	`output_low(MOT_L);`
617	`output_low(MOT_R);`	621	`output_low(MOT_R);`
618	`cas=CASAVR-CASMIN;`	622	`cas=CASAVR-CASMIN;`
619	`};`	623	`};`
620	`set_adc_channel(LMAX); // Levy UV sensor`	624	`set_adc_channel(LMAX); // Levy UV sensor`
621	`for(n=0;n<20;n++) if(input(HREF)) goto next_snap;`	625	`for(n=0;n<20;n++) if(input(HREF)) goto next_snap;`
622	`if(read_adc()<THR) cas=CASMIN;`	626	`if(read_adc()<THR) cas=CASMIN;`
623	`set_adc_channel(RMAX); // Pravy UV sensor`	627	`set_adc_channel(RMAX); // Pravy UV sensor`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 254 – Rev 249 → 252