Subversion Repositories svnkaklik

Rev

Rev 211 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 211 Rev 213
Line 101... Line 101...
101
 
101
 
102
if(cas==CASMIN)
102
if(cas==CASMIN)
103
{
103
{
104
   set_adc_channel(RMAX);    // Pravy UV sensor
104
   set_adc_channel(RMAX);    // Pravy UV sensor
105
   for(j=0;j<10000;j++)
105
   for(j=0;j<10000;j++)
106
   {   
106
   {
107
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
107
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
108
      if (read_adc()<128) break;     
108
      if (read_adc()<128) break;
109
   }
109
   }
110
}
110
}
111
else
111
else
112
{
112
{
113
   set_adc_channel(LMAX);    // Pravy UV sensor
113
   set_adc_channel(LMAX);    // Pravy UV sensor
114
   for(j=0;j<10000;j++)
114
   for(j=0;j<10000;j++)
115
   {   
115
   {
116
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
116
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
117
      if (read_adc()<128) break;     
117
      if (read_adc()<128) break;
118
   }
118
   }
119
}
119
}
120
 
120
 
121
set_adc_channel(RMAX);    // Pravy UV sensor
121
set_adc_channel(RMAX);    // Pravy UV sensor
122
 
122
 
123
set_pwm1_duty(0);    // zabrzdi
123
set_pwm1_duty(0);    // zabrzdi
124
set_pwm2_duty(0);
124
set_pwm2_duty(0);
125
output_high(MOT_L);
125
output_high(MOT_L);
126
output_high(MOT_R);
126
output_high(MOT_R);
127
SetServo(CASAVR-CASMIN);   // doprostred                
127
SetServo(CASAVR-CASMIN);   // doprostred
128
delay_ms(100);
128
delay_ms(100);
129
brzda();
129
brzda();
130
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
130
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
131
 
131
 
132
SetServo(CASMIN);   // max. doleva                 L
132
SetServo(CASMIN);   // max. doleva                 L
133
set_pwm1_duty(0);   // vzad
133
set_pwm1_duty(0);   // vzad
134
set_pwm2_duty(0);
134
set_pwm2_duty(0);
135
output_low(MOT_L);
135
output_low(MOT_L);
136
output_high(MOT_R);
136
output_high(MOT_R);
137
while (read_adc()>128);     
137
while (read_adc()>128);
138
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
138
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
139
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
139
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
140
set_pwm1_duty(0);    // reverz (zabrzdi)
140
set_pwm1_duty(0);    // reverz (zabrzdi)
141
set_pwm2_duty(255);
141
set_pwm2_duty(255);
142
output_low(MOT_L);
142
output_low(MOT_L);
143
output_low(MOT_R);
143
output_low(MOT_R);
144
SetServo(CASAVR-CASMIN);   // doprostred                
144
SetServo(CASAVR-CASMIN);   // doprostred
145
delay_ms(100);
145
delay_ms(100);
146
brzda();
146
brzda();
147
 
147
 
148
set_pwm1_duty(255);    // max. vpred
148
set_pwm1_duty(255);    // max. vpred
149
set_pwm2_duty(255);
149
set_pwm2_duty(255);