Subversion Repositories svnkaklik

Rev

Rev 210 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

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