Subversion Repositories svnkaklik

Rev

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

Rev 227 Rev 228
Line 7... Line 7...
7 
int8 n;
 7 
int8 n;
8 
int8 r1,r2,rr;
 8 
int8 r1,r2,rr;
9 
int8 touch;
 9 
int8 touch;
10 
enum okolo_cihly {pred_carou,na_care,po_care};
 10 
enum okolo_cihly {pred_carou,na_care,po_care};
11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
 11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
- 
 
 12 
int8 vzdalenost;
12 
 
 13 
 
13 
cihla:
 14 
cihla:
14 
 
 15 
 
15 
   rr=20;
 16 
   rr=20;
16 
   disp(0x99);
 17 
   disp(0x99);
Line 25... Line 26...
25
26
26 
      set_pwm1_duty(0);
 27 
      set_pwm1_duty(0);
27 
      set_pwm2_duty(200);
 28 
      set_pwm2_duty(200);
28 
      output_high(MOT_L);     // leve kolo reverz
 29 
      output_high(MOT_L);     // leve kolo reverz
29 
      output_low(MOT_R);      // prave kolo vpred
 30 
      output_low(MOT_R);      // prave kolo vpred
30 
      if(get_timer1()>(odocounter+8))  // konec zatacky?
 31 
      if(get_timer1()>(odocounter+5))  // konec zatacky?
31 
      {
 32 
      {
32 
         disp(0x66);
 33 
         disp(0x66);
33 
         cas=CASAVR-CASMIN;   // rovne
 - 
 
34 
         break;
 34 
         break;
35 
      }
 35 
      }
36 
      SetServoQ(cas);
 36 
      SetServoQ(cas);
37 
      delay_ms(18);
 37 
      delay_ms(18);
38 
   };
 38 
   };
39
39
40 
   //------ Objeti cihly v konstantni vzdalenosti ------
 40 
   //------ Objeti cihly v konstantni vzdalenosti ------
41 
   touch=0;
 41 
   touch=0;
42 
   ridic=pred_carou;
 42 
   ridic=pred_carou;
- 
 
 43 
   cas=CASAVR-CASMIN;   // rovne
43 
   output_low(MOT_L);   // vpred
 44 
   output_low(MOT_L);   // vpred
44 
   output_low(MOT_R);
 45 
   output_low(MOT_R);
45 
   while(true)
 46 
   while(true)
46 
   {
 47 
   {
47 
      if(!input(IRRX)) goto cihla;
 48 
//      if(!input(IRRX)) goto cihla;
48
49
49 
      if(!input(PROXIMITY))   // Udrzovani konstantni vzdalenosti od cihly
 50 
      if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
50 
      {
 51 
      {
51 
         if(cas>(CASMIN+20)) cas-=20;
 52 
         if(cas>(CASMIN+20)) cas-=20;
52 
      }
 53 
      }
53 
      else
 54 
      else
54 
      {
 55 
      {
Line 66... Line 67...
66 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
 67 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
67 
      set_pwm2_duty(r2);
 68 
      set_pwm2_duty(r2);
68
69
69 
      SetServoQ(cas);
 70 
      SetServoQ(cas);
70
71
- 
 
 72 
      i2c_start();     // Sonar Ping
- 
 
 73 
      i2c_write(0xE0);
- 
 
 74 
      i2c_write(0x0);
- 
 
 75 
      i2c_write(0x51);
- 
 
 76 
      i2c_stop();
- 
 
 77 
 
71 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
 78 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
72 
      {
 79 
      {
73 
         set_adc_channel(LMAX);
 80 
         set_adc_channel(LMAX);
74 
         delay_us(100);
 81 
         delay_us(100);
75 
         if(read_adc()<THR) touch|=L_TOUCH;
 82 
         if(read_adc()<THR) touch|=L_TOUCH;
76 
         set_adc_channel(RMAX);
 83 
         set_adc_channel(RMAX);
77 
         delay_us(100);
 84 
         delay_us(100);
78 
         if(read_adc()<THR) touch|=R_TOUCH;
 85 
         if(read_adc()<THR) touch|=R_TOUCH;
79 
      };
 86 
      };
- 
 
 87 
 
- 
 
 88 
      i2c_start();     // Odraz ze sonaru
- 
 
 89 
      i2c_write(0xE0);
- 
 
 90 
      i2c_write(0x3);
- 
 
 91 
      i2c_stop();
- 
 
 92 
      i2c_start();
- 
 
 93 
      i2c_write(0xE1);
- 
 
 94 
      vzdalenost=i2c_read(0);
- 
 
 95 
      i2c_stop();
- 
 
 96 
 
80 
      if(touch==L_TOUCH) disp(0xC0);
 97 
      if(touch==L_TOUCH) disp(0xC0);
81 
      if(touch==R_TOUCH) disp(0x03);
 98 
      if(touch==R_TOUCH) disp(0x03);
82 
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
 99 
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
83 
      if((ridic==na_care)&&(touch==0)) break;
 100 
      if((ridic==na_care)&&(touch==0)) break;
84 
      if(ridic==na_care) touch=0;
 101 
      if(ridic==na_care) touch=0;