Subversion Repositories svnkaklik

Rev

Details | 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 
 
226 kakl 13 
rr=20;
193 kakl 14 
 
226 kakl 15 
disp(0x99);
221 kakl 16 
odocounter=get_timer1();
226 kakl 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 
   {
27 
      disp(0x66);
28 
      cas=CASAVR-CASMIN;   // rovne
29 
      break;
30 
   }
31 
   SetServoQ(cas);
32 
   delay_ms(18);
33 
};
34 
 
35 
touch=0;
36 
ridic=pred_carou;
37 
output_low(MOT_L);   // vpred
38 
output_low(MOT_R);
193 kakl 39 
while(true)
40 
{
226 kakl 41 
 
42 
   if(!input(PROXIMITY))   // Udrzovani konstantni vzdalenosti od cihly
221 kakl 43 
   {
226 kakl 44 
      if(cas>(CASMIN+20)) cas-=20;
225 kakl 45 
   }
46 
   else
47 
   {
226 kakl 48 
      if(cas<(CASMAX-20)) cas+=20;
225 kakl 49 
   };
193 kakl 50 
 
225 kakl 51 
   // Elektronicky diferencial
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
193 kakl 55 
 
225 kakl 56 
   if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
57 
   if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
193 kakl 58 
 
226 kakl 59 
   set_pwm1_duty(r1);   // Nastav rychlost motoru
225 kakl 60 
   set_pwm2_duty(r2);
221 kakl 61 
 
226 kakl 62 
   SetServoQ(cas);
63 
 
225 kakl 64 
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
221 kakl 65 
   {
225 kakl 66 
      set_adc_channel(LMAX);
67 
      delay_us(100);
226 kakl 68 
      if(read_adc()<THR) touch|=L_TOUCH;
225 kakl 69 
      set_adc_channel(RMAX);
70 
      delay_us(100);
226 kakl 71 
      if(read_adc()<THR) touch|=R_TOUCH;
221 kakl 72 
   };
226 kakl 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;
221 kakl 78 
};
226 kakl 79 
disp(0xC3);
221 kakl 80 
 
226 kakl 81 
set_pwm1_duty(0);
223 kakl 82 
set_pwm2_duty(170);
226 kakl 83 
output_low(MOT_L);
223 kakl 84 
output_low(MOT_R);
225 kakl 85 
delay_us(40);
221 kakl 86 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
223 kakl 87 
while (true)  // Znovu se musime dotknout cary
88 
{
225 kakl 89 
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
223 kakl 90 
   {
226 kakl 91 
      set_adc_channel(LMAX);    // Levy UV sensor
92 
      delay_us(100);
93 
      if(read_adc()<THR)   // Dotkli jsme se levym senzorem
225 kakl 94 
      {
226 kakl 95 
         disp(0xE0);
225 kakl 96 
         cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
226 kakl 97 
         goto cara;
225 kakl 98 
      };
99 
      set_adc_channel(RMAX);    // Pravy UV sensor
100 
      delay_us(100);
226 kakl 101 
      if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
225 kakl 102 
      {
226 kakl 103 
         disp(0x07);
225 kakl 104 
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
226 kakl 105 
         goto cara;
225 kakl 106 
      };
107 
   }
108 
   SetServoQ(CASMIN);   // max. doleva                 L
223 kakl 109 
}
225 kakl 110 
 
226 kakl 111 
cara:
112 
;
113 
/*
221 kakl 114 
set_pwm1_duty(0);    // vypni motory
115 
set_pwm2_duty(0);
193 kakl 116 
 
221 kakl 117 
SetServo(CASAVR-CASMIN);   // doprostred
193 kakl 118 
 
221 kakl 119 
set_pwm1_duty(255);    // max. vpred
120 
set_pwm2_duty(255);
193 kakl 121 
output_low(MOT_L);
122 
output_low(MOT_R);
221 kakl 123 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
124 
while(get_timer1()<(odocounter+2)) // Ujed kousek
193 kakl 125 
{
221 kakl 126 
   set_adc_channel(LMAX);    // Levy UV sensor
127 
   delay_us(40);
226 kakl 128 
   if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
221 kakl 129 
   set_adc_channel(RMAX);    // Pravy UV sensor
130 
   delay_us(40);
226 kakl 131 
   if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
221 kakl 132 
};
226 kakl 133 
*/