Subversion Repositories svnkaklik

Rev

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

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