Subversion Repositories svnkaklik

Rev

Rev 226 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

Rev Author Line No. Line
193 kakl 1 
// **** Objeti cihly vlevo **** LLLL
2 
 
226 kakl 3 
#define L_TOUCH 1    // Cara vlevo
4 
#define R_TOUCH 2    // Cata vpravo
5 
#define B_TOUCH 3    // Both
6 
 
221 kakl 7 
int8 n;
8 
int8 r1,r2,rr;
226 kakl 9 
int8 touch;
10 
enum okolo_cihly {pred_carou,na_care,po_care};
11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
193 kakl 12 
 
227 kakl 13 
cihla:
193 kakl 14 
 
227 kakl 15 
   rr=20;
16 
   disp(0x99);
17 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
18 
   set_pwm2_duty(150);
19 
   output_high(MOT_L);
20 
   output_low(MOT_R);
21 
   odocounter=get_timer1();
22 
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
226 kakl 23 
   {
227 kakl 24 
      cas=CASMIN-5;       // jeste vic nez hodne do leva
25 
 
26 
      set_pwm1_duty(0);
27 
      set_pwm2_duty(200);
28 
      output_high(MOT_L);     // leve kolo reverz
29 
      output_low(MOT_R);      // prave kolo vpred
30 
      if(get_timer1()>(odocounter+8))  // konec zatacky?
31 
      {
32 
         disp(0x66);
33 
         cas=CASAVR-CASMIN;   // rovne
34 
         break;
35 
      }
36 
      SetServoQ(cas);
37 
      delay_ms(18);
225 kakl 38 
   };
227 kakl 39 
 
40 
   //------ Objeti cihly v konstantni vzdalenosti ------
41 
   touch=0;
42 
   ridic=pred_carou;
43 
   output_low(MOT_L);   // vpred
44 
   output_low(MOT_R);
45 
   while(true)
221 kakl 46 
   {
227 kakl 47 
      if(!input(IRRX)) goto cihla;
48 
 
49 
      if(!input(PROXIMITY))   // Udrzovani konstantni vzdalenosti od cihly
225 kakl 50 
      {
227 kakl 51 
         if(cas>(CASMIN+20)) cas-=20;
52 
      }
53 
      else
54 
      {
55 
         if(cas<(CASMAX-20)) cas+=20;
225 kakl 56 
      };
227 kakl 57 
 
58 
      // Elektronicky diferencial
59 
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
60 
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
61 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
62 
 
63 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
64 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
65 
 
66 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
67 
      set_pwm2_duty(r2);
68 
 
69 
      SetServoQ(cas);
70 
 
71 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
225 kakl 72 
      {
227 kakl 73 
         set_adc_channel(LMAX);
74 
         delay_us(100);
75 
         if(read_adc()<THR) touch|=L_TOUCH;
76 
         set_adc_channel(RMAX);
77 
         delay_us(100);
78 
         if(read_adc()<THR) touch|=R_TOUCH;
225 kakl 79 
      };
227 kakl 80 
      if(touch==L_TOUCH) disp(0xC0);
81 
      if(touch==R_TOUCH) disp(0x03);
82 
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
83 
      if((ridic==na_care)&&(touch==0)) break;
84 
      if(ridic==na_care) touch=0;
85 
   };
86 
   disp(0xC3);
87 
 
88 
   set_pwm1_duty(20);
89 
   set_pwm2_duty(200);
90 
   output_high(MOT_L);
91 
   output_low(MOT_R);
92 
   delay_us(40);
93 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
94 
   while (true)  // Znovu se musime dotknout cary
95 
   {
96 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
97 
      {
98 
         set_adc_channel(LMAX);    // Levy UV sensor
99 
         delay_us(100);
100 
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
101 
         {
102 
            disp(0xE0);
103 
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
104 
            goto cara;
105 
         };
106 
         set_adc_channel(RMAX);    // Pravy UV sensor
107 
         delay_us(100);
108 
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
109 
         {
110 
            disp(0x07);
111 
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
112 
            goto cara;
113 
         };
114 
      }
115 
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
225 kakl 116 
   }
227 kakl 117 
 
226 kakl 118 
cara:
227 kakl 119 
 
120 
   output_low(MOT_L); // oba motory vpred
121 
   output_low(MOT_R);
226 kakl 122 
/*
221 kakl 123 
set_pwm1_duty(0);    // vypni motory
124 
set_pwm2_duty(0);
193 kakl 125 
 
221 kakl 126 
SetServo(CASAVR-CASMIN);   // doprostred
193 kakl 127 
 
221 kakl 128 
set_pwm1_duty(255);    // max. vpred
129 
set_pwm2_duty(255);
193 kakl 130 
output_low(MOT_L);
131 
output_low(MOT_R);
221 kakl 132 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
133 
while(get_timer1()<(odocounter+2)) // Ujed kousek
193 kakl 134 
{
221 kakl 135 
   set_adc_channel(LMAX);    // Levy UV sensor
136 
   delay_us(40);
226 kakl 137 
   if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
221 kakl 138 
   set_adc_channel(RMAX);    // Pravy UV sensor
139 
   delay_us(40);
226 kakl 140 
   if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
221 kakl 141 
};
226 kakl 142 
*/