Subversion Repositories svnkaklik

Rev

Rev 214 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

Rev Author Line No. Line
193 kakl 1 
// **** Objeti cihly vpravo **** RRRR
208 kakl 2 
int8 n,i;
211 kakl 3 
int16 j;
193 kakl 4 
 
204 kakl 5 
set_adc_channel(RMAX);
6 
 
193 kakl 7 
SetServo(CASMIN);   // max. doleva                 L
8 
set_pwm1_duty(0);   // vzad
208 kakl 9 
set_pwm2_duty(0);
193 kakl 10 
output_low(MOT_L);
11 
output_high(MOT_R);
198 kakl 12 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
204 kakl 13 
while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost
193 kakl 14 
set_pwm1_duty(0);    // reverz (zabrzdi)
15 
set_pwm2_duty(255);
16 
output_low(MOT_L);
17 
output_low(MOT_R);
196 kakl 18 
delay_ms(215);
193 kakl 19 
brzda();
20 
 
207 kakl 21 
disp(1);
193 kakl 22 
SetServo((CASAVR-CASMIN));   // rovne              S
196 kakl 23 
set_pwm1_duty(160);  // vpred
24 
set_pwm2_duty(160);
193 kakl 25 
output_low(MOT_L);
26 
output_low(MOT_R);
198 kakl 27 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
208 kakl 28 
for(n=1;n<=7;n++)
29 
{
210 kakl 30 
  while(get_timer1()<(odocounter+n)); // Popojed
31 
  SetServoQ((CASAVR-CASMIN));
208 kakl 32 
};
193 kakl 33 
 
208 kakl 34 
i=0;
35 
 
207 kakl 36 
disp(2);
208 kakl 37 
set_pwm1_duty(130);
207 kakl 38 
set_pwm2_duty(140);
210 kakl 39 
odocounter=get_timer1();
208 kakl 40 
for(n=1;n<=10;n++)
41 
{
42 
   while(get_timer1()<(odocounter+n));
43 
   SetServoQ((CASAVR-CASMIN)-i);
44 
   set_pwm1_duty(130-i);
45 
   i+=8;
46 
};
193 kakl 47 
 
207 kakl 48 
disp(3);
210 kakl 49 
odocounter=get_timer1();
215 kakl 50 
for(n=1;n<=7;n++)
208 kakl 51 
{
52 
   while(get_timer1()<(odocounter+n));
53 
   SetServoQ((CASAVR-CASMIN)-i);
54 
   set_pwm1_duty(130-i);
55 
   i-=8;
56 
};
193 kakl 57 
 
207 kakl 58 
disp(4);
210 kakl 59 
odocounter=get_timer1();
215 kakl 60 
for(n=1;n<=7;n++)
208 kakl 61 
{
62 
   while(get_timer1()<(odocounter+n));
63 
   SetServoQ((CASAVR-CASMIN)-i);
64 
   set_pwm1_duty(130-i);
65 
   i+=8;
66 
};
200 kakl 67 
 
207 kakl 68 
disp(5);
210 kakl 69 
odocounter=get_timer1();
215 kakl 70 
//!!!!for(n=1;n<=5;n++)
71 
for(n=1;n<=4;n++)
208 kakl 72 
{
73 
   while(get_timer1()<(odocounter+n));
74 
   SetServoQ((CASAVR-CASMIN)-i);
75 
   set_pwm1_duty(130-i);
76 
   i-=16;
77 
};
193 kakl 78 
 
207 kakl 79 
disp(6);
208 kakl 80 
set_pwm1_duty(80);  // vpred
81 
set_pwm2_duty(80);
82 
output_low(MOT_L);
83 
output_low(MOT_R);
198 kakl 84 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
208 kakl 85 
for(n=1;n<=3;n++)
86 
{
87 
  while(get_timer1()<(odocounter+n));
210 kakl 88 
  SetServoQ((CASAVR-CASMIN));
208 kakl 89 
};
204 kakl 90 
 
207 kakl 91 
disp(7);
210 kakl 92 
while(true)
200 kakl 93 
{
204 kakl 94 
   set_adc_channel(LMAX);    // Levy UV sensor
95 
   delay_us(40);
215 kakl 96 
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
204 kakl 97 
   set_adc_channel(RMAX);    // Pravy UV sensor
98 
   delay_us(40);
215 kakl 99 
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; };  // Prejeli jsme caru vpravo
210 kakl 100 
}
200 kakl 101 
 
211 kakl 102 
 
210 kakl 103 
if(cas==CASMIN)
104 
{
105 
   set_adc_channel(RMAX);    // Pravy UV sensor
211 kakl 106 
   for(j=0;j<10000;j++)
213 kakl 107 
   {
211 kakl 108 
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
215 kakl 109 
      if (read_adc()<TRESHOLD) break;
211 kakl 110 
   }
210 kakl 111 
}
112 
else
113 
{
215 kakl 114 
   set_adc_channel(LMAX);    // Levy UV sensor
211 kakl 115 
   for(j=0;j<10000;j++)
213 kakl 116 
   {
211 kakl 117 
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
215 kakl 118 
      if (read_adc()<TRESHOLD) break;
211 kakl 119 
   }
210 kakl 120 
}
121 
 
211 kakl 122 
set_adc_channel(RMAX);    // Pravy UV sensor
210 kakl 123 
 
211 kakl 124 
set_pwm1_duty(0);    // zabrzdi
125 
set_pwm2_duty(0);
126 
output_high(MOT_L);
127 
output_high(MOT_R);
213 kakl 128 
SetServo(CASAVR-CASMIN);   // doprostred
211 kakl 129 
delay_ms(100);
130 
brzda();
131 
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
210 kakl 132 
 
211 kakl 133 
SetServo(CASMIN);   // max. doleva                 L
134 
set_pwm1_duty(0);   // vzad
135 
set_pwm2_duty(0);
136 
output_low(MOT_L);
137 
output_high(MOT_R);
215 kakl 138 
while (read_adc()>TRESHOLD);
211 kakl 139 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
140 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
141 
set_pwm1_duty(0);    // reverz (zabrzdi)
142 
set_pwm2_duty(255);
143 
output_low(MOT_L);
144 
output_low(MOT_R);
213 kakl 145 
SetServo(CASAVR-CASMIN);   // doprostred
211 kakl 146 
delay_ms(100);
147 
brzda();
210 kakl 148 
 
211 kakl 149 
set_pwm1_duty(255);    // max. vpred
150 
set_pwm2_duty(255);
151 
output_low(MOT_L);
152 
output_low(MOT_R);
153 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
154 
while(get_timer1()<(odocounter+2)) // Ujed kousek
155 
{
156 
   set_adc_channel(LMAX);    // Levy UV sensor
157 
   delay_us(40);
215 kakl 158 
   if(read_adc()<TRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
211 kakl 159 
   set_adc_channel(RMAX);    // Pravy UV sensor
160 
   delay_us(40);
215 kakl 161 
   if(read_adc()<TRESHOLD) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
211 kakl 162 
   cas=CASAVR-CASMIN;    // Cara je rovne
163 
};
164 
 
215 kakl 165 
stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
210 kakl 166 
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
167 
//rr=60;   // Nerozumna rychlost pro rozjeti