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 
*/