Subversion Repositories svnkaklik

Rev

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

Rev 210 Rev 211
Line 1... Line 1...
1 
// **** Objeti cihly vpravo **** RRRR
 1 
// **** Objeti cihly vpravo **** RRRR
2 
int8 n,i;
 2 
int8 n,i;
- 
 
 3 
int16 j;
3 
 
 4 
 
4 
set_adc_channel(RMAX);
 5 
set_adc_channel(RMAX);
5 
 
 6 
 
6 
SetServo(CASMIN);   // max. doleva                 L
 7 
SetServo(CASMIN);   // max. doleva                 L
7 
set_pwm1_duty(0);   // vzad
 8 
set_pwm1_duty(0);   // vzad
Line 95... Line 96...
95 
   set_adc_channel(RMAX);    // Pravy UV sensor
 96 
   set_adc_channel(RMAX);    // Pravy UV sensor
96 
   delay_us(40);
 97 
   delay_us(40);
97 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
 98 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
98 
}
 99 
}
99 
 
 100 
 
- 
 
 101 
 
100 
if(cas==CASMIN)
 102 
if(cas==CASMIN)
101 
{
 103 
{
102 
   set_adc_channel(RMAX);    // Pravy UV sensor
 104 
   set_adc_channel(RMAX);    // Pravy UV sensor
103 
   delay_us(40);
 105 
   for(j=0;j<10000;j++)
- 
 
 106 
   {
104 
   while(read_adc()>128);     // Prejel caru?
 107 
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
105 
   while(read_adc()<128);
 108 
      if (read_adc()<128) break;
- 
 
 109 
   }
106 
}
 110 
}
107 
else
 111 
else
108 
{
 112 
{
109 
   set_adc_channel(LMAX);    // Levy UV sensor
 113 
   set_adc_channel(LMAX);    // Pravy UV sensor
110 
   delay_us(40);
 114 
   for(j=0;j<10000;j++)
- 
 
 115 
   {
111 
   while(read_adc()>128);     // Prejel caru?
 116 
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
112 
   while(read_adc()<128);
 117 
      if (read_adc()<128) break;
- 
 
 118 
   }
113 
}
 119 
}
114 
// Tady jsou s velkou pravdepodobnosti obe cidla za carou
 - 
 
115 
 
 120 
 
116 
set_adc_channel(RMAX);
121 
set_adc_channel(RMAX);    // Pravy UV sensor
117 
SetServo(CASMAX-40);   // stredne doprava
 - 
 
118 
while(read_adc()>128);
 - 
 
119 
 
 122 
 
- 
 
 123 
set_pwm1_duty(0);    // zabrzdi
- 
 
 124 
set_pwm2_duty(0);
- 
 
 125 
output_high(MOT_L);
- 
 
 126 
output_high(MOT_R);
- 
 
 127 
SetServo(CASAVR-CASMIN);   // doprostred
- 
 
 128 
delay_ms(100);
- 
 
 129 
brzda();
- 
 
 130 
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
120 
 
 131 
 
- 
 
 132 
SetServo(CASMIN);   // max. doleva                 L
- 
 
 133 
set_pwm1_duty(0);   // vzad
- 
 
 134 
set_pwm2_duty(0);
- 
 
 135 
output_low(MOT_L);
- 
 
 136 
output_high(MOT_R);
- 
 
 137 
while (read_adc()>128);
- 
 
 138 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 139 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
- 
 
 140 
set_pwm1_duty(0);    // reverz (zabrzdi)
- 
 
 141 
set_pwm2_duty(255);
- 
 
 142 
output_low(MOT_L);
- 
 
 143 
output_low(MOT_R);
- 
 
 144 
SetServo(CASAVR-CASMIN);   // doprostred
- 
 
 145 
delay_ms(100);
- 
 
 146 
brzda();
- 
 
 147 
 
- 
 
 148 
set_pwm1_duty(255);    // max. vpred
- 
 
 149 
set_pwm2_duty(255);
- 
 
 150 
output_low(MOT_L);
- 
 
 151 
output_low(MOT_R);
- 
 
 152 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 153 
while(get_timer1()<(odocounter+2)) // Ujed kousek
- 
 
 154 
{
- 
 
 155 
   set_adc_channel(LMAX);    // Levy UV sensor
- 
 
 156 
   delay_us(40);
- 
 
 157 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
- 
 
 158 
   set_adc_channel(RMAX);    // Pravy UV sensor
- 
 
 159 
   delay_us(40);
- 
 
 160 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
121 
cas=CASAVR-CASMIN; // Cara je rovne
 161 
   cas=CASAVR-CASMIN;    // Cara je rovne
- 
 
 162 
};
122 
 
 163 
 
123 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
 164 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
124 
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 165 
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
125 
//rr=60;   // Nerozumna rychlost pro rozjeti
 166 
//rr=60;   // Nerozumna rychlost pro rozjeti