Subversion Repositories svnkaklik

Rev

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

Rev 223 Rev 225
Line 4... Line 4...
4 
int8 r1,r2,rr;
 4 
int8 r1,r2,rr;
5 
int1 flag;
 5 
int1 flag;
6 
 
 6 
 
7 
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
 7 
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
8 
set_pwm1_duty(0);
 8 
set_pwm1_duty(0);
9 
set_pwm2_duty(140);
 9 
set_pwm2_duty(200);
10 
output_low(MOT_L);    // vpred
 10 
output_low(MOT_L);    // vpred
11 
output_low(MOT_R);
 11 
output_low(MOT_R);
12 
disp(0x81);
 12 
disp(0x81);
13 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 13 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
14 
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
 14 
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
15 
 
 15 
 
16 
set_pwm1_duty(80);
 16 
set_pwm1_duty(80);   // zpomal
17 
set_pwm2_duty(80);
 17 
set_pwm2_duty(80);
18 
set_adc_channel(LMAX);
 18 
set_adc_channel(LMAX);
19 
disp(0x82);
 19 
disp(0x82);
20 
cas=CASAVR-CASMIN;
 20 
cas=CASAVR-CASMIN;         // rovne
21 
odocounter=get_timer1();
 21 
odocounter=get_timer1();
22 
flag=1;
 22 
flag=1;
23 
rr=20;
 23 
rr=20;
24 
while(true)
 24 
while(true)
25 
{
 25 
{
26 
   if(input(HREF))
 26 
   if(!input(PROXIMITY))
27 
   {
 27 
   {
28 
      if(!input(PROXIMITY))
 - 
 
29 
      {
 - 
 
30 
         if(cas>(CASMIN+10)) cas-=10;
 28 
      if(cas>(CASMIN+10)) cas-=10;
31 
      }
 29 
   }
32 
      else
 30 
   else
33 
      {
 31 
   {
34 
         if(cas<(CASMAX-10)) cas+=10;
 32 
      if(cas<(CASMAX-10)) cas+=10;
35 
      };
 33 
   };
36 
 
 34 
 
37 
      SetServoQ(cas);
 35 
   SetServoQ(cas);
38 
 
 36 
 
39 
      // Elektronicky diferencial
 37 
   // Elektronicky diferencial
40 
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
 38 
   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
 39 
   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
 40 
   if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
43 
 
 41 
 
44 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
 42 
   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);
 43 
   if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
46 
 
 44 
 
47 
      set_pwm1_duty(r1);
 45 
   set_pwm1_duty(r1);
48 
      set_pwm2_duty(r2);
 46 
   set_pwm2_duty(r2);
49 
 
 47 
 
50 
      while(input(HREF));
 - 
 
51 
   };
 - 
 
52 
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
 48 
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
53 
   {
 49 
   {
- 
 
 50 
/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
- 
 
 51 
      if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
- 
 
 52 
      {
54 
      if(flag==1) {disp(0x83); flag=0;};
 53 
         if(flag==1) {disp(0x83); 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;};}; // Neni to jen skvira nebo spina?
 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);
- 
 
 58 
      delay_us(100);
- 
 
 59 
      if(read_adc()<THR_L) {set_adc_channel(RMAX); delay_us(40); disp(0x13); goto cara1;};
- 
 
 60 
      set_adc_channel(RMAX);
- 
 
 61 
      delay_us(100);
- 
 
 62 
      if(read_adc()<THR_L) {set_adc_channel(LMAX); delay_us(40); disp(0x23); goto cara1;};
56 
   };
 63 
   };
57 
};
 64 
};
58 
 
 65 
 
- 
 
 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
59 
disp(0x84);
 73 
disp(0x84);
60 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 - 
 
61 
SetServo(CASAVR);    // Primy smer
 - 
 
62 
while((read_adc()<THR_L) || (get_timer1()<(odocounter+2))); // Popojed za caru
 - 
 
63 
 
 74 
 
- 
 
 75 
set_adc_channel(LMAX);
64 
SetServo(CASMIN);   // max. doleva                 L
 76 
// SetServo(CASMIN);   // max. doleva                 L
65 
set_pwm1_duty(20);
 77 
set_pwm1_duty(20);
66 
set_pwm2_duty(170);
 78 
set_pwm2_duty(170);
67 
output_high(MOT_L);    // levym kolem brzdi
 79 
output_high(MOT_L);    // levym kolem brzdi
68 
output_low(MOT_R);
 80 
output_low(MOT_R);
- 
 
 81 
delay_us(40);
69 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 82 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
70 
while (true)  // Znovu se musime dotknout cary
 83 
while (true)  // Znovu se musime dotknout cary
71 
{
 84 
{
72 
   if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
 85 
   for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
73 
   {
 - 
 
74 
      while(read_adc()<THR_L); // Prejedeme caru
 - 
 
75 
      cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
 - 
 
76 
      break;
 - 
 
77 
   };
 - 
 
78 
   set_adc_channel(RMAX);    // Pravy UV sensor
 - 
 
79 
   delay_us(40);
 - 
 
80 
   if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
 - 
 
81 
   {
 86 
   {
- 
 
 87 
      if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
- 
 
 88 
      {
- 
 
 89 
         while(read_adc()<THR_L); // Prejedeme caru
- 
 
 90 
         cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
- 
 
 91 
         goto cara2;
- 
 
 92 
      };
- 
 
 93 
      set_adc_channel(RMAX);    // Pravy UV sensor
- 
 
 94 
      delay_us(100);
- 
 
 95 
      if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
- 
 
 96 
      {
82 
      cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
 97 
         cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
83 
      break;
 98 
         goto cara2;
84 
   };
 99 
      };
85 
   set_adc_channel(LMAX);    // Levy UV sensor
 100 
      set_adc_channel(LMAX);    // Levy UV sensor
86 
   delay_us(40);
 101 
      delay_us(100);
- 
 
 102 
   }
- 
 
 103 
   SetServoQ(CASMIN);   // max. doleva                 L
87 
}
 104 
}
- 
 
 105 
 
- 
 
 106 
cara2:
- 
 
 107 
 
88 
set_pwm1_duty(0);    // vypni motory
 108 
set_pwm1_duty(0);    // vypni motory
89 
set_pwm2_duty(0);
 109 
set_pwm2_duty(0);
90 
 
 110 
 
91 
SetServo(CASAVR-CASMIN);   // doprostred
 111 
SetServo(CASAVR-CASMIN);   // doprostred
92 
 
 112 
 
Line 102... Line 122...
102 
   delay_us(40);
 122 
   delay_us(40);
103 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
 123 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
104 
   set_adc_channel(RMAX);    // Pravy UV sensor
 124 
   set_adc_channel(RMAX);    // Pravy UV sensor
105 
   delay_us(40);
 125 
   delay_us(40);
106 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
 126 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
107 
//   cas=CASAVR-CASMIN;    // Cara je rovne
 - 
 
108 
};
 127 
};