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;
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();
208 kakl 50 
for(n=1;n<=6;n++)
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();
208 kakl 60 
for(n=1;n<=6;n++)
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();
208 kakl 70 
for(n=1;n<=5;n++)
71 
{
72 
   while(get_timer1()<(odocounter+n));
73 
   SetServoQ((CASAVR-CASMIN)-i);
74 
   set_pwm1_duty(130-i);
75 
   i-=16;
76 
};
193 kakl 77 
 
207 kakl 78 
disp(6);
208 kakl 79 
set_pwm1_duty(80);  // vpred
80 
set_pwm2_duty(80);
81 
output_low(MOT_L);
82 
output_low(MOT_R);
198 kakl 83 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
208 kakl 84 
for(n=1;n<=3;n++)
85 
{
86 
  while(get_timer1()<(odocounter+n));
210 kakl 87 
  SetServoQ((CASAVR-CASMIN));
208 kakl 88 
};
204 kakl 89 
 
207 kakl 90 
disp(7);
210 kakl 91 
while(true)
200 kakl 92 
{
204 kakl 93 
   set_adc_channel(LMAX);    // Levy UV sensor
94 
   delay_us(40);
95 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
96 
   set_adc_channel(RMAX);    // Pravy UV sensor
97 
   delay_us(40);
98 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
210 kakl 99 
}
200 kakl 100 
 
211 kakl 101 
 
210 kakl 102 
if(cas==CASMIN)
103 
{
104 
   set_adc_channel(RMAX);    // Pravy UV sensor
211 kakl 105 
   for(j=0;j<10000;j++)
106 
   {
107 
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
108 
      if (read_adc()<128) break;
109 
   }
210 kakl 110 
}
111 
else
112 
{
211 kakl 113 
   set_adc_channel(LMAX);    // Pravy UV sensor
114 
   for(j=0;j<10000;j++)
115 
   {
116 
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
117 
      if (read_adc()<128) break;
118 
   }
210 kakl 119 
}
120 
 
211 kakl 121 
set_adc_channel(RMAX);    // Pravy UV sensor
210 kakl 122 
 
211 kakl 123 
set_pwm1_duty(0);    // zabrzdi
124 
set_pwm2_duty(0);
125 
output_high(MOT_L);
126 
output_high(MOT_R);
127 
SetServo(CASAVR-CASMIN);   // doprostred
128 
delay_ms(100);
129 
brzda();
130 
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
210 kakl 131 
 
211 kakl 132 
SetServo(CASMIN);   // max. doleva                 L
133 
set_pwm1_duty(0);   // vzad
134 
set_pwm2_duty(0);
135 
output_low(MOT_L);
136 
output_high(MOT_R);
137 
while (read_adc()>128);
138 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
139 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
140 
set_pwm1_duty(0);    // reverz (zabrzdi)
141 
set_pwm2_duty(255);
142 
output_low(MOT_L);
143 
output_low(MOT_R);
144 
SetServo(CASAVR-CASMIN);   // doprostred
145 
delay_ms(100);
146 
brzda();
210 kakl 147 
 
211 kakl 148 
set_pwm1_duty(255);    // max. vpred
149 
set_pwm2_duty(255);
150 
output_low(MOT_L);
151 
output_low(MOT_R);
152 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
153 
while(get_timer1()<(odocounter+2)) // Ujed kousek
154 
{
155 
   set_adc_channel(LMAX);    // Levy UV sensor
156 
   delay_us(40);
157 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
158 
   set_adc_channel(RMAX);    // Pravy UV sensor
159 
   delay_us(40);
160 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
161 
   cas=CASAVR-CASMIN;    // Cara je rovne
162 
};
163 
 
207 kakl 164 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
210 kakl 165 
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
166 
//rr=60;   // Nerozumna rychlost pro rozjeti