Subversion Repositories svnkaklik

Rev

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

Rev 217 Rev 221
Line 1... Line 1...
1
// **** Objeti cihly vpravo **** RRRR
1
// **** Objeti cihly vpravo **** RRRR
2
// Verze pouzita na Robot Challenge 
-
 
3
 
2
 
4
int8 n,i;
3
int8 n,i;
5
int16 j;
4
int16 j;
6
 
5
 
7
set_adc_channel(RMAX);
6
set_adc_channel(RMAX);
Line 33... Line 32...
33
  SetServoQ((CASAVR-CASMIN));
32
  SetServoQ((CASAVR-CASMIN));
34
};
33
};
35
 
34
 
36
i=0;
35
i=0;
37
 
36
 
38
disp(2);
37
disp(2);                   // Postupne doleva
39
set_pwm1_duty(130);
38
set_pwm1_duty(130);
40
set_pwm2_duty(140);
39
set_pwm2_duty(140);
41
odocounter=get_timer1();
40
odocounter=get_timer1();
42
for(n=1;n<=10;n++)
41
for(n=1;n<=10;n++)
43
{
42
{
Line 45... Line 44...
45
   SetServoQ((CASAVR-CASMIN)-i);
44
   SetServoQ((CASAVR-CASMIN)-i);
46
   set_pwm1_duty(130-i);
45
   set_pwm1_duty(130-i);
47
   i+=8;
46
   i+=8;
48
};
47
};
49
 
48
 
-
 
49
/*
50
disp(3);
50
disp(3);
51
odocounter=get_timer1();
51
odocounter=get_timer1();
52
for(n=1;n<=7;n++)
52
for(n=1;n<=7;n++)
53
{
53
{
54
   while(get_timer1()<(odocounter+n));
54
   while(get_timer1()<(odocounter+n));
Line 64... Line 64...
64
   while(get_timer1()<(odocounter+n));
64
   while(get_timer1()<(odocounter+n));
65
   SetServoQ((CASAVR-CASMIN)-i);
65
   SetServoQ((CASAVR-CASMIN)-i);
66
   set_pwm1_duty(130-i);
66
   set_pwm1_duty(130-i);
67
   i+=8;
67
   i+=8;
68
};
68
};
-
 
69
*/
69
 
70
 
70
disp(5);
71
disp(5);
71
odocounter=get_timer1();
72
odocounter=get_timer1();
72
//!!!!for(n=1;n<=5;n++)
73
//!!!!for(n=1;n<=5;n++)
73
for(n=1;n<=4;n++)
74
for(n=1;n<=4;n++)
Line 93... Line 94...
93
disp(7);
94
disp(7);
94
while(true)
95
while(true)
95
{
96
{
96
   set_adc_channel(LMAX);    // Levy UV sensor
97
   set_adc_channel(LMAX);    // Levy UV sensor
97
   delay_us(40);
98
   delay_us(40);
98
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
99
   if(read_adc()<THRESHOLD) {delay_us(600); if(read_adc()<THRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
99
   set_adc_channel(RMAX);    // Pravy UV sensor
100
   set_adc_channel(RMAX);    // Pravy UV sensor
100
   delay_us(40);
101
   delay_us(40);
101
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; };  // Prejeli jsme caru vpravo
102
   if(read_adc()<THRESHOLD) {delay_us(600); if(read_adc()<THRESHOLD) {cas=CASMAX; break;}; };  // Prejeli jsme caru vpravo
102
}
103
}
103
 
104
 
104
 
105
 
105
if(cas==CASMIN)
106
if(cas==CASMIN)
106
{
107
{
107
   set_adc_channel(RMAX);    // Pravy UV sensor
108
   set_adc_channel(RMAX);    // Pravy UV sensor
108
   for(j=0;j<10000;j++)
109
   for(j=0;j<10000;j++)
109
   {
110
   {
110
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
111
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
111
      if (read_adc()<TRESHOLD) break;
112
      if (read_adc()<THRESHOLD) break;
112
   }
113
   }
113
}
114
}
114
else
115
else
115
{
116
{
116
   set_adc_channel(LMAX);    // Levy UV sensor
117
   set_adc_channel(LMAX);    // Levy UV sensor
117
   for(j=0;j<10000;j++)
118
   for(j=0;j<10000;j++)
118
   {
119
   {
119
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
120
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
120
      if (read_adc()<TRESHOLD) break;
121
      if (read_adc()<THRESHOLD) break;
121
   }
122
   }
122
}
123
}
123
 
124
 
124
set_adc_channel(RMAX);    // Pravy UV sensor
125
set_adc_channel(RMAX);    // Pravy UV sensor
125
 
126
 
Line 135... Line 136...
135
SetServo(CASMIN);   // max. doleva                 L
136
SetServo(CASMIN);   // max. doleva                 L
136
set_pwm1_duty(0);   // vzad
137
set_pwm1_duty(0);   // vzad
137
set_pwm2_duty(0);
138
set_pwm2_duty(0);
138
output_low(MOT_L);
139
output_low(MOT_L);
139
output_high(MOT_R);
140
output_high(MOT_R);
140
while (read_adc()>TRESHOLD);
141
while (read_adc()>THRESHOLD);
141
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
142
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
142
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
143
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
143
set_pwm1_duty(0);    // reverz (zabrzdi)
144
set_pwm1_duty(0);    // reverz (zabrzdi)
144
set_pwm2_duty(255);
145
set_pwm2_duty(255);
145
output_low(MOT_L);
146
output_low(MOT_L);
Line 155... Line 156...
155
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
156
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
156
while(get_timer1()<(odocounter+2)) // Ujed kousek
157
while(get_timer1()<(odocounter+2)) // Ujed kousek
157
{
158
{
158
   set_adc_channel(LMAX);    // Levy UV sensor
159
   set_adc_channel(LMAX);    // Levy UV sensor
159
   delay_us(40);
160
   delay_us(40);
160
   if(read_adc()<TRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
161
   if(read_adc()<THRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
161
   set_adc_channel(RMAX);    // Pravy UV sensor
162
   set_adc_channel(RMAX);    // Pravy UV sensor
162
   delay_us(40);
163
   delay_us(40);
163
   if(read_adc()<TRESHOLD) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
164
   if(read_adc()<THRESHOLD) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
164
   cas=CASAVR-CASMIN;    // Cara je rovne
165
   cas=CASAVR-CASMIN;    // Cara je rovne
165
};
166
};
166
 
167
 
167
stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
168
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
168
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
169
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
169
//rr=60;   // Nerozumna rychlost pro rozjeti
170
//rr=60;   // Nerozumna rychlost pro rozjeti