Subversion Repositories svnkaklik

Rev

Rev 207 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log

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