Subversion Repositories svnkaklik

Rev

Rev 223 | 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 
 
221 kakl 3 
int8 n;
4 
int8 r1,r2,rr;
5 
int1 flag;
193 kakl 6 
 
223 kakl 7 
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
221 kakl 8 
set_pwm1_duty(0);
225 kakl 9 
set_pwm2_duty(200);
221 kakl 10 
output_low(MOT_L);    // vpred
193 kakl 11 
output_low(MOT_R);
223 kakl 12 
disp(0x81);
221 kakl 13 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
225 kakl 14 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
193 kakl 15 
 
225 kakl 16 
set_pwm1_duty(80);   // zpomal
221 kakl 17 
set_pwm2_duty(80);
18 
set_adc_channel(LMAX);
223 kakl 19 
disp(0x82);
225 kakl 20 
cas=CASAVR-CASMIN;         // rovne
221 kakl 21 
odocounter=get_timer1();
22 
flag=1;
23 
rr=20;
193 kakl 24 
while(true)
25 
{
225 kakl 26 
   if(!input(PROXIMITY))
221 kakl 27 
   {
225 kakl 28 
      if(cas>(CASMIN+10)) cas-=10;
29 
   }
30 
   else
31 
   {
32 
      if(cas<(CASMAX-10)) cas+=10;
33 
   };
193 kakl 34 
 
225 kakl 35 
   SetServoQ(cas);
193 kakl 36 
 
225 kakl 37 
   // Elektronicky diferencial
38 
   if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
39 
   if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
40 
   if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
193 kakl 41 
 
225 kakl 42 
   if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
43 
   if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
193 kakl 44 
 
225 kakl 45 
   set_pwm1_duty(r1);
46 
   set_pwm2_duty(r2);
221 kakl 47 
 
225 kakl 48 
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
221 kakl 49 
   {
225 kakl 50 
/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
51 
      if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
52 
      {
53 
         if(flag==1) {disp(0x83); flag=0;};
54 
         if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina?
55 
      };
56 
*/
57 
      set_adc_channel(LMAX);
58 
      delay_us(100);
59 
      if(read_adc()<THR_L) {set_adc_channel(RMAX); delay_us(40); disp(0x13); goto cara1;};
60 
      set_adc_channel(RMAX);
61 
      delay_us(100);
62 
      if(read_adc()<THR_L) {set_adc_channel(LMAX); delay_us(40); disp(0x23); goto cara1;};
221 kakl 63 
   };
64 
};
65 
 
225 kakl 66 
cara1:
67 
 
68 
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
69 
//!!! SetServo(CASAVR);    // Primy smer !!!!!!!!!!!! to se nestiha
70 
//set_pwm1_duty(140);   // zrychli
71 
//set_pwm2_duty(140);
72 
while(read_adc()>THR_L); // I druhym senzorem dojed k care
223 kakl 73 
disp(0x84);
221 kakl 74 
 
225 kakl 75 
set_adc_channel(LMAX);
76 
// SetServo(CASMIN);   // max. doleva                 L
223 kakl 77 
set_pwm1_duty(20);
78 
set_pwm2_duty(170);
79 
output_high(MOT_L);    // levym kolem brzdi
80 
output_low(MOT_R);
225 kakl 81 
delay_us(40);
221 kakl 82 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
223 kakl 83 
while (true)  // Znovu se musime dotknout cary
84 
{
225 kakl 85 
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
223 kakl 86 
   {
225 kakl 87 
      if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
88 
      {
89 
         while(read_adc()<THR_L); // Prejedeme caru
90 
         cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
91 
         goto cara2;
92 
      };
93 
      set_adc_channel(RMAX);    // Pravy UV sensor
94 
      delay_us(100);
95 
      if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
96 
      {
97 
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
98 
         goto cara2;
99 
      };
100 
      set_adc_channel(LMAX);    // Levy UV sensor
101 
      delay_us(100);
102 
   }
103 
   SetServoQ(CASMIN);   // max. doleva                 L
223 kakl 104 
}
225 kakl 105 
 
106 
cara2:
107 
 
221 kakl 108 
set_pwm1_duty(0);    // vypni motory
109 
set_pwm2_duty(0);
193 kakl 110 
 
221 kakl 111 
SetServo(CASAVR-CASMIN);   // doprostred
193 kakl 112 
 
223 kakl 113 
disp(0x85);
221 kakl 114 
set_pwm1_duty(255);    // max. vpred
115 
set_pwm2_duty(255);
193 kakl 116 
output_low(MOT_L);
117 
output_low(MOT_R);
221 kakl 118 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
119 
while(get_timer1()<(odocounter+2)) // Ujed kousek
193 kakl 120 
{
221 kakl 121 
   set_adc_channel(LMAX);    // Levy UV sensor
122 
   delay_us(40);
123 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
124 
   set_adc_channel(RMAX);    // Pravy UV sensor
125 
   delay_us(40);
126 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
127 
};