Subversion Repositories svnkaklik

Rev

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