Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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