Subversion Repositories svnkaklik

Rev

Rev 208 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 208 Rev 210
Line 24... Line 24...
24 
output_low(MOT_L);
 24 
output_low(MOT_L);
25 
output_low(MOT_R);
 25 
output_low(MOT_R);
26 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 26 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
27 
for(n=1;n<=7;n++)
 27 
for(n=1;n<=7;n++)
28 
{
 28 
{
29 
  while(get_timer1()<(odocounter+n)); // Popojed
29 
  while(get_timer1()<(odocounter+n)); // Popojed
30 
  SetServoQ((CASAVR-CASMIN));
30 
  SetServoQ((CASAVR-CASMIN));
31 
};
 31 
};
32 
 
 32 
 
33 
i=0;
 33 
i=0;
34 
 
 34 
 
35 
disp(2);
 35 
disp(2);
36 
set_pwm1_duty(130);
 36 
set_pwm1_duty(130);
37 
set_pwm2_duty(140);
 37 
set_pwm2_duty(140);
38 
odocounter=get_timer1();
38 
odocounter=get_timer1();
39 
for(n=1;n<=10;n++)
 39 
for(n=1;n<=10;n++)
40 
{
 40 
{
41 
   while(get_timer1()<(odocounter+n));
 41 
   while(get_timer1()<(odocounter+n));
42 
   SetServoQ((CASAVR-CASMIN)-i);
 42 
   SetServoQ((CASAVR-CASMIN)-i);
43 
   set_pwm1_duty(130-i);
 43 
   set_pwm1_duty(130-i);
44 
   i+=8;
 44 
   i+=8;
45 
};
 45 
};
46 
 
 46 
 
47 
disp(3);
 47 
disp(3);
48 
odocounter=get_timer1();
48 
odocounter=get_timer1();
49 
for(n=1;n<=6;n++)
 49 
for(n=1;n<=6;n++)
50 
{
 50 
{
51 
   while(get_timer1()<(odocounter+n));
 51 
   while(get_timer1()<(odocounter+n));
52 
   SetServoQ((CASAVR-CASMIN)-i);
 52 
   SetServoQ((CASAVR-CASMIN)-i);
53 
   set_pwm1_duty(130-i);
 53 
   set_pwm1_duty(130-i);
54 
   i-=8;
 54 
   i-=8;
55 
};
 55 
};
56 
 
 56 
 
57 
disp(4);
 57 
disp(4);
58 
odocounter=get_timer1();
58 
odocounter=get_timer1();
59 
for(n=1;n<=6;n++)
 59 
for(n=1;n<=6;n++)
60 
{
 60 
{
61 
   while(get_timer1()<(odocounter+n));
 61 
   while(get_timer1()<(odocounter+n));
62 
   SetServoQ((CASAVR-CASMIN)-i);
 62 
   SetServoQ((CASAVR-CASMIN)-i);
63 
   set_pwm1_duty(130-i);
 63 
   set_pwm1_duty(130-i);
64 
   i+=8;
 64 
   i+=8;
65 
};
 65 
};
66 
 
 66 
 
67 
disp(5);
 67 
disp(5);
68 
odocounter=get_timer1();
68 
odocounter=get_timer1();
69 
for(n=1;n<=5;n++)
 69 
for(n=1;n<=5;n++)
70 
{
 70 
{
71 
   while(get_timer1()<(odocounter+n));
 71 
   while(get_timer1()<(odocounter+n));
72 
   SetServoQ((CASAVR-CASMIN)-i);
 72 
   SetServoQ((CASAVR-CASMIN)-i);
73 
   set_pwm1_duty(130-i);
 73 
   set_pwm1_duty(130-i);
Line 81... Line 81...
81 
output_low(MOT_R);
 81 
output_low(MOT_R);
82 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 82 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
83 
for(n=1;n<=3;n++)
 83 
for(n=1;n<=3;n++)
84 
{
 84 
{
85 
  while(get_timer1()<(odocounter+n));
 85 
  while(get_timer1()<(odocounter+n));
86 
  SetServoQ((CASAVR-CASMIN));
86 
  SetServoQ((CASAVR-CASMIN));
87 
};
 87 
};
88 
 
 88 
 
89 
disp(7);
 89 
disp(7);
90 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 - 
 
91 
while(get_timer1()<(odocounter+20)) // Popojed definovanou vzdalenos
 - 
 
92 
{
 - 
 
93 
   if(read_adc()<128) break; // Neprejeli jsme caru?
 - 
 
94 
};
 - 
 
95 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
96 
set_pwm2_duty(0);
 - 
 
97 
output_high(MOT_L);
 - 
 
98 
output_high(MOT_R);
 - 
 
99 
delay_ms(300);
 - 
 
100 
brzda();
 - 
 
101 
 
 - 
 
102 
disp(8);
 - 
 
103 
SetServo(CASMIN);   // max. doleva                 L
 - 
 
104 
set_pwm1_duty(0);   // vzad
 - 
 
105 
set_pwm2_duty(0);
 - 
 
106 
output_low(MOT_L);
 - 
 
107 
output_high(MOT_R);
 - 
 
108 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 - 
 
109 
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost
 - 
 
110 
{
 - 
 
111 
   if(read_adc()<128) break; // Neprejeli jsme caru?
 - 
 
112 
};
 - 
 
113 
 
 - 
 
114 
disp(9);
 - 
 
115 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 - 
 
116 
while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly
 - 
 
117 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
118 
set_pwm2_duty(255);
 - 
 
119 
output_low(MOT_L);
 - 
 
120 
output_low(MOT_R);
 - 
 
121 
delay_ms(150);
 - 
 
122 
brzda();
 - 
 
123 
 
 - 
 
124 
disp(10);
 90 
while(true)
125 
SetServo((CASAVR-CASMIN));   // rovne              S
 - 
 
126 
set_pwm1_duty(255);  // Rozjezd na plny vykon vpred
 - 
 
127 
set_pwm2_duty(255);
 - 
 
128 
output_low(MOT_L);
 - 
 
129 
output_low(MOT_R);
 - 
 
130 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 - 
 
131 
while(get_timer1()<(odocounter+2)) // Uz jedeme? (prekonani sertvacne sily)
 - 
 
132 
{
 91 
{
133 
   set_adc_channel(LMAX);    // Levy UV sensor
 92 
   set_adc_channel(LMAX);    // Levy UV sensor
134 
   delay_us(40);
 93 
   delay_us(40);
135 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
 94 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
136 
   set_adc_channel(RMAX);    // Pravy UV sensor
 95 
   set_adc_channel(RMAX);    // Pravy UV sensor
137 
   delay_us(40);
 96 
   delay_us(40);
138 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
 97 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
- 
 
 98 
}
- 
 
 99 
 
- 
 
 100 
if(cas==CASMIN)
- 
 
 101 
{
139 
   cas=CASAVR-CASMIN;    // Cara je rovne
 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);
140 
};
 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
141 
 
 122 
 
142 
disp(11);
 - 
 
143 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
 123 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
144 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 124 
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
145 
rr=60;   // Nerozumna rychlost pro rozjeti
 125 
//rr=60;   // Nerozumna rychlost pro rozjeti