Subversion Repositories svnkaklik

Rev

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

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 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 546... Line 549...
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