Subversion Repositories svnkaklik

Rev

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

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