Subversion Repositories svnkaklik

Rev

Rev 225 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 225 Rev 226
1
// **** Objeti cihly vlevo **** LLLL
1
// **** Objeti cihly vlevo **** LLLL
2
 
2
 
-
 
3
#define L_TOUCH 1    // Cara vlevo
-
 
4
#define R_TOUCH 2    // Cata vpravo
-
 
5
#define B_TOUCH 3    // Both
-
 
6
 
3
int8 n;
7
int8 n;
4
int8 r1,r2,rr;
8
int8 r1,r2,rr;
5
int1 flag;
9
int8 touch;
-
 
10
enum okolo_cihly {pred_carou,na_care,po_care};
-
 
11
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
6
 
12
 
7
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
-
 
8
set_pwm1_duty(0);
-
 
9
set_pwm2_duty(200);
-
 
10
output_low(MOT_L);    // vpred
-
 
11
output_low(MOT_R);
-
 
12
disp(0x81);
13
rr=20;
13
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
-
 
14
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
-
 
15
 
14
 
16
set_pwm1_duty(80);   // zpomal
-
 
17
set_pwm2_duty(80);
-
 
18
set_adc_channel(LMAX);
-
 
19
disp(0x82);
15
disp(0x99);
20
cas=CASAVR-CASMIN;         // rovne
-
 
21
odocounter=get_timer1();
16
odocounter=get_timer1();
-
 
17
while(true) // Na zacatku se vyhni cihle, zatoc co muzes
-
 
18
{
-
 
19
   cas=CASMIN-5;       // jeste vic nez hodne do leva
-
 
20
 
-
 
21
   set_pwm1_duty(0);
-
 
22
   set_pwm2_duty(200);
-
 
23
   output_high(MOT_L);     // leve kolo reverz
-
 
24
   output_low(MOT_R);      // prave kolo vpred
-
 
25
   if(get_timer1()>(odocounter+5))  // konec zatacky?
-
 
26
   {
-
 
27
      disp(0x66);
-
 
28
      cas=CASAVR-CASMIN;   // rovne
-
 
29
      break;
-
 
30
   }
-
 
31
   SetServoQ(cas);
22
flag=1;
32
   delay_ms(18);
-
 
33
};
-
 
34
 
23
rr=20;
35
touch=0;
-
 
36
ridic=pred_carou;
-
 
37
output_low(MOT_L);   // vpred
-
 
38
output_low(MOT_R);
24
while(true)
39
while(true)
25
{
40
{
-
 
41
 
26
   if(!input(PROXIMITY))
42
   if(!input(PROXIMITY))   // Udrzovani konstantni vzdalenosti od cihly
27
   {
43
   {
28
      if(cas>(CASMIN+10)) cas-=10;
44
      if(cas>(CASMIN+20)) cas-=20;
29
   }
45
   }
30
   else
46
   else
31
   {
47
   {
32
      if(cas<(CASMAX-10)) cas+=10;
48
      if(cas<(CASMAX-20)) cas+=20;
33
   };
49
   };
34
 
50
 
35
   SetServoQ(cas);
-
 
36
 
-
 
37
   // Elektronicky diferencial
51
   // Elektronicky diferencial
38
   if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
52
   if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
39
   if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
53
   if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
40
   if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
54
   if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
41
 
55
 
42
   if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
56
   if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
43
   if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
57
   if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
44
 
58
 
45
   set_pwm1_duty(r1);
59
   set_pwm1_duty(r1);   // Nastav rychlost motoru
46
   set_pwm2_duty(r2);
60
   set_pwm2_duty(r2);
47
 
61
 
-
 
62
   SetServoQ(cas);
-
 
63
 
48
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
64
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
49
   {
65
   {
50
/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-
 
51
      if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
-
 
52
      {
-
 
53
         if(flag==1) {disp(0x83); flag=0;};
-
 
54
         if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina?
-
 
55
      };
-
 
56
*/
-
 
57
      set_adc_channel(LMAX);
66
      set_adc_channel(LMAX);
58
      delay_us(100);
67
      delay_us(100);
59
      if(read_adc()<THR_L) {set_adc_channel(RMAX); delay_us(40); disp(0x13); goto cara1;};
68
      if(read_adc()<THR) touch|=L_TOUCH;
60
      set_adc_channel(RMAX);
69
      set_adc_channel(RMAX);
61
      delay_us(100);
70
      delay_us(100);
62
      if(read_adc()<THR_L) {set_adc_channel(LMAX); delay_us(40); disp(0x23); goto cara1;};
71
      if(read_adc()<THR) touch|=R_TOUCH;
63
   };
72
   };
-
 
73
   if(touch==L_TOUCH) disp(0xC0);
-
 
74
   if(touch==R_TOUCH) disp(0x03);
-
 
75
   if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care; 
-
 
76
   if((ridic==na_care)&&(touch==0)) break;
-
 
77
   if(ridic==na_care) touch=0;
64
};
78
};
-
 
79
disp(0xC3);
65
 
80
 
66
cara1:
-
 
67
 
-
 
68
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
-
 
69
//!!! SetServo(CASAVR);    // Primy smer !!!!!!!!!!!! to se nestiha
-
 
70
//set_pwm1_duty(140);   // zrychli
-
 
71
//set_pwm2_duty(140);
-
 
72
while(read_adc()>THR_L); // I druhym senzorem dojed k care
-
 
73
disp(0x84);
-
 
74
 
-
 
75
set_adc_channel(LMAX);
-
 
76
// SetServo(CASMIN);   // max. doleva                 L
-
 
77
set_pwm1_duty(20);
81
set_pwm1_duty(0);
78
set_pwm2_duty(170);
82
set_pwm2_duty(170);
79
output_high(MOT_L);    // levym kolem brzdi
83
output_low(MOT_L);
80
output_low(MOT_R);
84
output_low(MOT_R);
81
delay_us(40);
85
delay_us(40);
82
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
86
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
83
while (true)  // Znovu se musime dotknout cary
87
while (true)  // Znovu se musime dotknout cary
84
{
88
{
85
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
89
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
86
   {
90
   {
-
 
91
      set_adc_channel(LMAX);    // Levy UV sensor
-
 
92
      delay_us(100);
87
      if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
93
      if(read_adc()<THR)   // Dotkli jsme se levym senzorem
88
      {
94
      {
89
         while(read_adc()<THR_L); // Prejedeme caru
95
         disp(0xE0);
90
         cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
96
         cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
91
         goto cara2;
97
         goto cara;
92
      };
98
      };
93
      set_adc_channel(RMAX);    // Pravy UV sensor
99
      set_adc_channel(RMAX);    // Pravy UV sensor
94
      delay_us(100);
100
      delay_us(100);
95
      if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
101
      if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
96
      {
102
      {
-
 
103
         disp(0x07);
97
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
104
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
98
         goto cara2;
105
         goto cara;
99
      };
106
      };
100
      set_adc_channel(LMAX);    // Levy UV sensor
-
 
101
      delay_us(100);
-
 
102
   }
107
   }
103
   SetServoQ(CASMIN);   // max. doleva                 L
108
   SetServoQ(CASMIN);   // max. doleva                 L
104
}
109
}
105
 
110
 
106
cara2:
111
cara:
107
 
112
;
-
 
113
/*
108
set_pwm1_duty(0);    // vypni motory
114
set_pwm1_duty(0);    // vypni motory
109
set_pwm2_duty(0);
115
set_pwm2_duty(0);
110
 
116
 
111
SetServo(CASAVR-CASMIN);   // doprostred
117
SetServo(CASAVR-CASMIN);   // doprostred
112
 
118
 
113
disp(0x85);
-
 
114
set_pwm1_duty(255);    // max. vpred
119
set_pwm1_duty(255);    // max. vpred
115
set_pwm2_duty(255);
120
set_pwm2_duty(255);
116
output_low(MOT_L);
121
output_low(MOT_L);
117
output_low(MOT_R);
122
output_low(MOT_R);
118
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
123
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
119
while(get_timer1()<(odocounter+2)) // Ujed kousek
124
while(get_timer1()<(odocounter+2)) // Ujed kousek
120
{
125
{
121
   set_adc_channel(LMAX);    // Levy UV sensor
126
   set_adc_channel(LMAX);    // Levy UV sensor
122
   delay_us(40);
127
   delay_us(40);
123
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
128
   if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
124
   set_adc_channel(RMAX);    // Pravy UV sensor
129
   set_adc_channel(RMAX);    // Pravy UV sensor
125
   delay_us(40);
130
   delay_us(40);
126
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
131
   if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
127
};
132
};
-
 
133
*/