Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
231 kaklik 1
// **** Objeti cihly vlevo **** LLLL
2
 
3
int8 n;
4
int8 r1,r2,rr;
5
int1 flag;
6
 
7
SetServo(CASMAX);   // max. doleva, zatoc kolmo na caru
8
set_pwm1_duty(0);
9
set_pwm2_duty(0);
10
output_high(MOT_L);    // vzad
11
output_low(MOT_R);
12
disp(1);
13
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
14
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
15
set_pwm1_duty(255);    // reverz (zabrzdi)
16
set_pwm2_duty(0);
17
output_low(MOT_L);
18
output_low(MOT_R);
19
delay_ms(150);
20
brzda();
21
 
22
set_pwm1_duty(80);
23
set_pwm2_duty(80);
24
set_adc_channel(LMAX);
25
disp(2);
26
cas=CASAVR-CASMIN;
27
odocounter=get_timer1();
28
flag=1;
29
rr=20;
30
while(true)
31
{
32
   if(input(HREF))
33
   {
34
      if(!input(PROXIMITY))
35
      {
36
         if(cas>(CASMIN+10)) cas-=10;
37
      }
38
      else
39
      {
40
         if(cas<(CASMAX-10)) cas+=10;
41
      };
42
 
43
      SetServoQ(cas);
44
 
45
      // Elektronicky diferencial
46
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
47
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
48
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
49
 
50
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
51
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
52
 
53
      set_pwm1_duty(r1);
54
      set_pwm2_duty(r2);
55
 
56
      while(input(HREF));
57
   };
58
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
59
   {
60
      if(flag==1) {disp(3); flag=0;};
61
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};};
62
   };
63
};
64
 
65
disp(4);
66
while (read_adc()<THR_L);
67
 
68
SetServo(CASMIN);   // max. doleva                 L
69
set_pwm1_duty(0);
70
set_pwm2_duty(140);
71
while (read_adc()>THR_L);
72
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
73
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
74
set_pwm1_duty(0);    // vypni motory
75
set_pwm2_duty(0);
76
/*
77
set_pwm1_duty(0);    // reverz (zabrzdi)
78
set_pwm2_duty(0);
79
output_low(MOT_L);
80
output_high(MOT_R);
81
*/
82
 
83
SetServo(CASAVR-CASMIN);   // doprostred
84
 
85
disp(5);
86
set_pwm1_duty(255);    // max. vpred
87
set_pwm2_duty(255);
88
output_low(MOT_L);
89
output_low(MOT_R);
90
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
91
while(get_timer1()<(odocounter+2)) // Ujed kousek
92
{
93
   set_adc_channel(LMAX);    // Levy UV sensor
94
   delay_us(40);
95
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
96
   set_adc_channel(RMAX);    // Pravy UV sensor
97
   delay_us(40);
98
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
99
   cas=CASAVR-CASMIN;    // Cara je rovne
100
};