Subversion Repositories svnkaklik

Rev

Rev 226 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

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