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 
{
29 
  while(get_timer1()<(odocounter+n)); // Popojed
30 
  SetServoQ((CASAVR-CASMIN));
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);
208 kakl 38 
odocounter=get_timer1();
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);
208 kakl 48 
odocounter=get_timer1();
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);
208 kakl 58 
odocounter=get_timer1();
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);
208 kakl 68 
odocounter=get_timer1();
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));
86 
  SetServoQ((CASAVR-CASMIN));
87 
};
204 kakl 88 
 
207 kakl 89 
disp(7);
204 kakl 90 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
208 kakl 91 
while(get_timer1()<(odocounter+20)) // Popojed definovanou vzdalenos
200 kakl 92 
{
204 kakl 93 
   if(read_adc()<128) break; // Neprejeli jsme caru?
94 
};
193 kakl 95 
set_pwm1_duty(0);    // reverz (zabrzdi)
96 
set_pwm2_duty(0);
97 
output_high(MOT_L);
98 
output_high(MOT_R);
204 kakl 99 
delay_ms(300);
193 kakl 100 
brzda();
101 
 
208 kakl 102 
disp(8);
200 kakl 103 
SetServo(CASMIN);   // max. doleva                 L
104 
set_pwm1_duty(0);   // vzad
208 kakl 105 
set_pwm2_duty(0);
196 kakl 106 
output_low(MOT_L);
200 kakl 107 
output_high(MOT_R);
108 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
109 
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost
110 
{
204 kakl 111 
   if(read_adc()<128) break; // Neprejeli jsme caru?
200 kakl 112 
};
207 kakl 113 
 
208 kakl 114 
disp(9);
202 kakl 115 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
204 kakl 116 
while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly
200 kakl 117 
set_pwm1_duty(0);    // reverz (zabrzdi)
118 
set_pwm2_duty(255);
119 
output_low(MOT_L);
196 kakl 120 
output_low(MOT_R);
208 kakl 121 
delay_ms(150);
200 kakl 122 
brzda();
196 kakl 123 
 
208 kakl 124 
disp(10);
200 kakl 125 
SetServo((CASAVR-CASMIN));   // rovne              S
204 kakl 126 
set_pwm1_duty(255);  // Rozjezd na plny vykon vpred
200 kakl 127 
set_pwm2_duty(255);
128 
output_low(MOT_L);
129 
output_low(MOT_R);
204 kakl 130 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
131 
while(get_timer1()<(odocounter+2)) // Uz jedeme? (prekonani sertvacne sily)
132 
{
133 
   set_adc_channel(LMAX);    // Levy UV sensor
134 
   delay_us(40);
135 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
136 
   set_adc_channel(RMAX);    // Pravy UV sensor
137 
   delay_us(40);
138 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
139 
   cas=CASAVR-CASMIN;    // Cara je rovne
140 
};
200 kakl 141 
 
208 kakl 142 
disp(11);
207 kakl 143 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
204 kakl 144 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
145 
rr=60;   // Nerozumna rychlost pro rozjeti