Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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