Subversion Repositories svnkaklik

Rev

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

Rev 254 Rev 255
Line 18... Line 18...
18 
cihla:
 18 
cihla:
19 
 
 19 
 
20 
   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
 20 
   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
21 
   disp(0x99);
 21 
   disp(0x99);
22 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
 22 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
23 
   set_pwm2_duty(255);
 23 
   set_pwm2_duty(220);
24 
   output_high(MOT_L);
 24 
   output_high(MOT_L);
25 
   output_low(MOT_R);
 25 
   output_low(MOT_R);
26 
   odocounter=get_timer1();
 26 
   odocounter=get_timer1();
27 
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
 27 
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
28 
   {
 28 
   {
Line 36... Line 36...
36 
         brzda();
 36 
         brzda();
37 
         goto cihla; // Znovu zacni cihlu objizdet
 37 
         goto cihla; // Znovu zacni cihlu objizdet
38 
      };
 38 
      };
39 
 
 39 
 
40 
      set_pwm1_duty(0);
 40 
      set_pwm1_duty(0);
41 
      set_pwm2_duty(255);     // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
 41 
      set_pwm2_duty(220);     // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
42 
      output_high(MOT_L);     // leve kolo reverz
 42 
      output_high(MOT_L);     // leve kolo reverz
43 
      output_low(MOT_R);      // prave kolo vpred
 43 
      output_low(MOT_R);      // prave kolo vpred
44 
      if(get_timer1()>(odocounter+5))  // konec zatacky?
 44 
      if(get_timer1()>(odocounter+5))  // konec zatacky?
45 
      {
 45 
      {
46 
         disp(0x66);
 46 
         disp(0x66);
Line 94... Line 94...
94 
 
 94 
 
95 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
 95 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
96 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 96 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
97 
 
 97 
 
98 
//!!! pro zatuhle prevodovky
 98 
//!!! pro zatuhle prevodovky
99 
      r1<<=1;     // Rychlost je dvojnasobna
 99 
//      r1<<=1;     // Rychlost je dvojnasobna
100 
      r2<<=1;     // Rozsah 2 az 184 pro rr=0
 100 
//      r2<<=1;     // Rozsah 2 az 184 pro rr=0
101 
 
 101 
 
102 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
 102 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
103 
      set_pwm2_duty(r2);
 103 
      set_pwm2_duty(r2);
104 
 
 104 
 
105 
      SetServoQ(cas);
 105 
      SetServoQ(cas);
Line 145... Line 145...
145 
      if(ridic==na_care) touch=0;
 145 
      if(ridic==na_care) touch=0;
146 
      disp(visualisation);
 146 
      disp(visualisation);
147 
   };
 147 
   };
148 
   disp(0xC3);
 148 
   disp(0xC3);
149 
 
 149 
 
150 
   set_pwm1_duty(0);    //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
 150 
   set_pwm1_duty(20);    //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
151 
   set_pwm2_duty(255);
 151 
   set_pwm2_duty(250);
152 
   output_high(MOT_L);
 152 
   output_high(MOT_L);
153 
   output_low(MOT_R);
 153 
   output_low(MOT_R);
154 
   delay_us(40);
 154 
   delay_us(40);
155 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 155 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
156 
   while (true)  // Znovu se musime dotknout cary
 156 
   while (true)  // Znovu se musime dotknout cary