Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
193 kakl 1
// **** Objeti cihly vpravo **** RRRR
217 kakl 2
// Verze pouzita na Robot Challenge 
3
 
208 kakl 4
int8 n,i;
211 kakl 5
int16 j;
193 kakl 6
 
204 kakl 7
set_adc_channel(RMAX);
8
 
193 kakl 9
SetServo(CASMIN);   // max. doleva                 L
10
set_pwm1_duty(0);   // vzad
208 kakl 11
set_pwm2_duty(0);
193 kakl 12
output_low(MOT_L);
13
output_high(MOT_R);
198 kakl 14
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
204 kakl 15
while(get_timer1()<(odocounter+8)); // Popojed definovanou vzdalenost
193 kakl 16
set_pwm1_duty(0);    // reverz (zabrzdi)
17
set_pwm2_duty(255);
18
output_low(MOT_L);
19
output_low(MOT_R);
196 kakl 20
delay_ms(215);
193 kakl 21
brzda();
22
 
207 kakl 23
disp(1);
193 kakl 24
SetServo((CASAVR-CASMIN));   // rovne              S
196 kakl 25
set_pwm1_duty(160);  // vpred
26
set_pwm2_duty(160);
193 kakl 27
output_low(MOT_L);
28
output_low(MOT_R);
198 kakl 29
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
208 kakl 30
for(n=1;n<=7;n++)
31
{
210 kakl 32
  while(get_timer1()<(odocounter+n)); // Popojed
33
  SetServoQ((CASAVR-CASMIN));
208 kakl 34
};
193 kakl 35
 
208 kakl 36
i=0;
37
 
207 kakl 38
disp(2);
208 kakl 39
set_pwm1_duty(130);
207 kakl 40
set_pwm2_duty(140);
210 kakl 41
odocounter=get_timer1();
208 kakl 42
for(n=1;n<=10;n++)
43
{
44
   while(get_timer1()<(odocounter+n));
45
   SetServoQ((CASAVR-CASMIN)-i);
46
   set_pwm1_duty(130-i);
47
   i+=8;
48
};
193 kakl 49
 
207 kakl 50
disp(3);
210 kakl 51
odocounter=get_timer1();
215 kakl 52
for(n=1;n<=7;n++)
208 kakl 53
{
54
   while(get_timer1()<(odocounter+n));
55
   SetServoQ((CASAVR-CASMIN)-i);
56
   set_pwm1_duty(130-i);
57
   i-=8;
58
};
193 kakl 59
 
207 kakl 60
disp(4);
210 kakl 61
odocounter=get_timer1();
215 kakl 62
for(n=1;n<=7;n++)
208 kakl 63
{
64
   while(get_timer1()<(odocounter+n));
65
   SetServoQ((CASAVR-CASMIN)-i);
66
   set_pwm1_duty(130-i);
67
   i+=8;
68
};
200 kakl 69
 
207 kakl 70
disp(5);
210 kakl 71
odocounter=get_timer1();
215 kakl 72
//!!!!for(n=1;n<=5;n++)
73
for(n=1;n<=4;n++)
208 kakl 74
{
75
   while(get_timer1()<(odocounter+n));
76
   SetServoQ((CASAVR-CASMIN)-i);
77
   set_pwm1_duty(130-i);
78
   i-=16;
79
};
193 kakl 80
 
207 kakl 81
disp(6);
208 kakl 82
set_pwm1_duty(80);  // vpred
83
set_pwm2_duty(80);
84
output_low(MOT_L);
85
output_low(MOT_R);
198 kakl 86
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
208 kakl 87
for(n=1;n<=3;n++)
88
{
89
  while(get_timer1()<(odocounter+n));
210 kakl 90
  SetServoQ((CASAVR-CASMIN));
208 kakl 91
};
204 kakl 92
 
207 kakl 93
disp(7);
210 kakl 94
while(true)
200 kakl 95
{
204 kakl 96
   set_adc_channel(LMAX);    // Levy UV sensor
97
   delay_us(40);
215 kakl 98
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
204 kakl 99
   set_adc_channel(RMAX);    // Pravy UV sensor
100
   delay_us(40);
215 kakl 101
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; };  // Prejeli jsme caru vpravo
210 kakl 102
}
200 kakl 103
 
211 kakl 104
 
210 kakl 105
if(cas==CASMIN)
106
{
107
   set_adc_channel(RMAX);    // Pravy UV sensor
211 kakl 108
   for(j=0;j<10000;j++)
213 kakl 109
   {
211 kakl 110
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
215 kakl 111
      if (read_adc()<TRESHOLD) break;
211 kakl 112
   }
210 kakl 113
}
114
else
115
{
215 kakl 116
   set_adc_channel(LMAX);    // Levy UV sensor
211 kakl 117
   for(j=0;j<10000;j++)
213 kakl 118
   {
211 kakl 119
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
215 kakl 120
      if (read_adc()<TRESHOLD) break;
211 kakl 121
   }
210 kakl 122
}
123
 
211 kakl 124
set_adc_channel(RMAX);    // Pravy UV sensor
210 kakl 125
 
211 kakl 126
set_pwm1_duty(0);    // zabrzdi
127
set_pwm2_duty(0);
128
output_high(MOT_L);
129
output_high(MOT_R);
213 kakl 130
SetServo(CASAVR-CASMIN);   // doprostred
211 kakl 131
delay_ms(100);
132
brzda();
133
// Tady jsou s velkou pravdepodobnosti obe cidla za carou a jsme kolmo k care
210 kakl 134
 
211 kakl 135
SetServo(CASMIN);   // max. doleva                 L
136
set_pwm1_duty(0);   // vzad
137
set_pwm2_duty(0);
138
output_low(MOT_L);
139
output_high(MOT_R);
217 kakl 140
while (read_adc()>TRESHOLD);
211 kakl 141
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
142
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
143
set_pwm1_duty(0);    // reverz (zabrzdi)
144
set_pwm2_duty(255);
145
output_low(MOT_L);
146
output_low(MOT_R);
213 kakl 147
SetServo(CASAVR-CASMIN);   // doprostred
211 kakl 148
delay_ms(100);
149
brzda();
210 kakl 150
 
211 kakl 151
set_pwm1_duty(255);    // max. vpred
152
set_pwm2_duty(255);
153
output_low(MOT_L);
154
output_low(MOT_R);
155
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
156
while(get_timer1()<(odocounter+2)) // Ujed kousek
157
{
158
   set_adc_channel(LMAX);    // Levy UV sensor
159
   delay_us(40);
215 kakl 160
   if(read_adc()<TRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
211 kakl 161
   set_adc_channel(RMAX);    // Pravy UV sensor
162
   delay_us(40);
215 kakl 163
   if(read_adc()<TRESHOLD) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
211 kakl 164
   cas=CASAVR-CASMIN;    // Cara je rovne
165
};
166
 
215 kakl 167
stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
210 kakl 168
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
169
//rr=60;   // Nerozumna rychlost pro rozjeti