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
};