Subversion Repositories svnkaklik

Rev

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