Subversion Repositories svnkaklik

Rev

Rev 202 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 202 Rev 204
Line 1... Line 1...
1 
// **** Objeti cihly vpravo **** RRRR
 1 
// **** Objeti cihly vpravo **** RRRR
2 
 
 2 
 
- 
 
 3 
set_adc_channel(RMAX);
- 
 
 4 
 
3 
SetServo(CASMIN);   // max. doleva                 L
 5 
SetServo(CASMIN);   // max. doleva                 L
4 
set_pwm1_duty(0);   // vzad
 6 
set_pwm1_duty(0);   // vzad
5 
set_pwm2_duty(10);
 7 
set_pwm2_duty(10);
6 
output_low(MOT_L);
 8 
output_low(MOT_L);
7 
output_high(MOT_R);
 9 
output_high(MOT_R);
8 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 10 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
9 
while(get_timer1()<(odocounter+10)); // Popojed definovanou vzdalenost
 11 
while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost
10 
set_pwm1_duty(0);    // reverz (zabrzdi)
 12 
set_pwm1_duty(0);    // reverz (zabrzdi)
11 
set_pwm2_duty(255);
 13 
set_pwm2_duty(255);
12 
output_low(MOT_L);
 14 
output_low(MOT_L);
13 
output_low(MOT_R);
 15 
output_low(MOT_R);
14 
delay_ms(215);
 16 
delay_ms(215);
Line 18... Line 20...
18 
set_pwm1_duty(160);  // vpred
 20 
set_pwm1_duty(160);  // vpred
19 
set_pwm2_duty(160);
 21 
set_pwm2_duty(160);
20 
output_low(MOT_L);
 22 
output_low(MOT_L);
21 
output_low(MOT_R);
 23 
output_low(MOT_R);
22 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 24 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
23 
while(get_timer1()<(odocounter+10)); // Popojed definovanou vzdalenost
 25 
while(get_timer1()<(odocounter+5)); // Popojed definovanou vzdalenost
24 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
25 
set_pwm2_duty(0);
 - 
 
26 
output_high(MOT_L);
 - 
 
27 
output_high(MOT_R);
 - 
 
28 
delay_ms(250);
 - 
 
29 
brzda();
 - 
 
30 
 
 26 
 
31 
SetServo(CASMIN);   // max. doleva                 L
 - 
 
32 
set_pwm1_duty(0);  // vpred
 - 
 
33 
set_pwm2_duty(220);
 27 
SetServoQ((CASAVR-CASMIN)-20);
34 
output_low(MOT_L);
 - 
 
35 
output_low(MOT_R);
 - 
 
36 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 28 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
37 
while(get_timer1()<(odocounter+7)); // Popojed definovanou vzdalenost
 29 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
38 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
39 
set_pwm2_duty(0);
 30 
set_pwm1_duty(140);
40 
output_low(MOT_L);
 - 
 
41 
output_high(MOT_R);
 - 
 
42 
delay_ms(250);
 31 
set_pwm1_duty(120);
43 
brzda();
 - 
 
44 
 
 32 
 
45 
SetServo((CASAVR-CASMIN));   // rovne              S
 33 
SetServoQ((CASAVR-CASMIN)-30);
46 
set_pwm1_duty(160);  // vpred
 - 
 
47 
set_pwm2_duty(160);
 - 
 
48 
output_low(MOT_L);
 - 
 
49 
output_low(MOT_R);
 - 
 
50 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 34 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
51 
while(get_timer1()<(odocounter+10)); // Popojed definovanou vzdalenost
 35 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
52 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
53 
set_pwm2_duty(0);
 36 
set_pwm1_duty(100);
54 
output_high(MOT_L);
 - 
 
55 
output_high(MOT_R);
 - 
 
56 
delay_ms(250);
 - 
 
57 
brzda();
 - 
 
58 
 
 37 
 
59 
SetServo(CASMIN);   // max. doleva                 L
 - 
 
60 
set_pwm1_duty(0);  // vpred
 38 
SetServoQ((CASAVR-CASMIN)-60);
61 
set_pwm2_duty(220);
 - 
 
62 
output_low(MOT_L);
 - 
 
63 
output_low(MOT_R);
 - 
 
64 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 39 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
65 
while(get_timer1()<(odocounter+7)); // Popojed definovanou vzdalenost
 40 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
66 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
67 
set_pwm2_duty(0);
 41 
set_pwm1_duty(80);
68 
output_low(MOT_L);
 - 
 
69 
output_high(MOT_R);
 - 
 
70 
delay_ms(250);
 - 
 
71 
 
 42 
 
- 
 
 43 
SetServoQ((CASAVR-CASMIN)-40);
72 
set_adc_channel(RMAX);    // A/D prevodnik na cteni praveho UV cidla
 44 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 45 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
73 
brzda();
 46 
set_pwm1_duty(80);
74 
 
 47 
 
75 
SetServo((CASAVR-CASMIN));   // rovne              S
 48 
SetServoQ((CASAVR-CASMIN)-70);
76 
set_pwm1_duty(160);  // vpred
 - 
 
77 
set_pwm2_duty(160);
 - 
 
78 
output_low(MOT_L);
 - 
 
79 
output_low(MOT_R);
 - 
 
80 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 49 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
81 
while(get_timer1()<(odocounter+9)) // Popojed definovanou vzdalenost
 50 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
- 
 
 51 
set_pwm1_duty(80);
82 
{
 52 
 
- 
 
 53 
SetServoQ((CASAVR-CASMIN)-60);
- 
 
 54 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
83 
   if(read_adc()<128) break; // Neprejeli jsem caru?
 55 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
- 
 
 56 
set_pwm1_duty(100);
84 
};
57 
 
- 
 
 58 
SetServoQ((CASAVR-CASMIN)-30);
85 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 59 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
86 
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
 60 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
- 
 
 61 
set_pwm1_duty(120);
- 
 
 62 
 
- 
 
 63 
SetServoQ((CASAVR-CASMIN));		// Rovne
- 
 
 64 
set_pwm1_duty(80);  // vpred
- 
 
 65 
set_pwm2_duty(80);
- 
 
 66 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 67 
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenos
- 
 
 68 
{
- 
 
 69 
   if(read_adc()<128) break; // Neprejeli jsme caru?
- 
 
 70 
};
87 
set_pwm1_duty(0);    // reverz (zabrzdi)
 71 
set_pwm1_duty(0);    // reverz (zabrzdi)
88 
set_pwm2_duty(0);
 72 
set_pwm2_duty(0);
89 
output_high(MOT_L);
 73 
output_high(MOT_L);
90 
output_high(MOT_R);
 74 
output_high(MOT_R);
91 
delay_ms(250);
 75 
delay_ms(300);
92 
brzda();
 76 
brzda();
93 
 
 77 
 
94 
SetServo(CASMIN);   // max. doleva                 L
 78 
SetServo(CASMIN);   // max. doleva                 L
95 
set_pwm1_duty(0);   // vzad
 79 
set_pwm1_duty(0);   // vzad
96 
set_pwm2_duty(10);
 80 
set_pwm2_duty(10);
97 
output_low(MOT_L);
 81 
output_low(MOT_L);
98 
output_high(MOT_R);
 82 
output_high(MOT_R);
99 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 83 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
100 
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost
 84 
while(get_timer1()<(odocounter+10)) // Popojed definovanou vzdalenost
101 
{
 85 
{
102 
   if(read_adc()<128) break; // Neprejeli jsem caru?
 86 
   if(read_adc()<128) break; // Neprejeli jsme caru?
103 
};
 87 
};
104 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 88 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
105 
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
 89 
while(get_timer1()<(odocounter+3)); // Jeste trochu doprava, aby byla cara mezi krajnimi cidly
106 
set_pwm1_duty(0);    // reverz (zabrzdi)
 90 
set_pwm1_duty(0);    // reverz (zabrzdi)
107 
set_pwm2_duty(255);
 91 
set_pwm2_duty(255);
108 
output_low(MOT_L);
 92 
output_low(MOT_L);
109 
output_low(MOT_R);
 93 
output_low(MOT_R);
110 
delay_ms(200);
 94 
delay_ms(200);
111 
brzda();
 95 
brzda();
112 
 
 96 
 
113 
SetServo((CASAVR-CASMIN));   // rovne              S
 97 
SetServo((CASAVR-CASMIN));   // rovne              S
114 
set_pwm1_duty(255);  // vpred
 98 
set_pwm1_duty(255);  // Rozjezd na plny vykon vpred
115 
set_pwm2_duty(255);
 99 
set_pwm2_duty(255);
116 
output_low(MOT_L);
 100 
output_low(MOT_L);
117 
output_low(MOT_R);
 101 
output_low(MOT_R);
- 
 
 102 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 103 
while(get_timer1()<(odocounter+2)) // Uz jedeme? (prekonani sertvacne sily)
- 
 
 104 
{
- 
 
 105 
   set_adc_channel(LMAX);    // Levy UV sensor
- 
 
 106 
   delay_us(40);
- 
 
 107 
   if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
- 
 
 108 
   set_adc_channel(RMAX);    // Pravy UV sensor
- 
 
 109 
   delay_us(40);
- 
 
 110 
   if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
- 
 
 111 
   cas=CASAVR-CASMIN;    // Cara je rovne
- 
 
 112 
};
118 
 
 113 
 
119 
cas=CASAVR-CASMIN;    // Cara je rovne
 114 
stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
120 
 
 - 
 
121 
//!!!!!!!!!!!!!!!!!  stav=cihla;    // Stav po objeti cihly, uz zadna cihla nebude
 115 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 116 
rr=60;   // Nerozumna rychlost pro rozjeti