Subversion Repositories svnkaklik

Rev

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

Rev 238 Rev 239
1
// **** Objeti cihly vlevo **** LLLL
1
// **** Objeti cihly vlevo **** LLLL
2
 
2
 
3
#define L_TOUCH 1    // Cara vlevo
3
#define L_TOUCH 1    // Cara vlevo
4
#define R_TOUCH 2    // Cata vpravo
4
#define R_TOUCH 2    // Cata vpravo
5
#define B_TOUCH 3    // Both
5
#define B_TOUCH 3    // Both
6
 
6
 
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
int8 vzdalenost;
13
 
13
 
14
   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
14
   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
15
   touch=0;    // Indikator detekce cary pri objizdeni
15
   touch=0;    // Indikator detekce cary pri objizdeni
16
 
16
 
17
cihla:
17
cihla:
18
 
18
 
19
   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
19
   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
20
   disp(0x99);
20
   disp(0x99);
21
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
21
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
22
   set_pwm2_duty(150);
22
   set_pwm2_duty(200);
23
   output_high(MOT_L);
23
   output_high(MOT_L);
24
   output_low(MOT_R);
24
   output_low(MOT_R);
25
   odocounter=get_timer1();
25
   odocounter=get_timer1();
26
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
26
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
27
   {
27
   {
28
      cas=CASMIN-5;       // jeste vic nez hodne do leva
28
      cas=CASMIN-5;       // jeste vic nez hodne do leva
29
 
29
 
30
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
30
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
31
      {
31
      {
32
         bum();
32
         bum();
33
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
33
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
-
 
34
         brzda();
34
         goto cihla; // Znovu zacni cihlu objizdet
35
         goto cihla; // Znovu zacni cihlu objizdet
35
      };
36
      };
36
 
37
 
37
      set_pwm1_duty(0);
38
      set_pwm1_duty(0);
38
      set_pwm2_duty(200);
39
      set_pwm2_duty(200);
39
      output_high(MOT_L);     // leve kolo reverz
40
      output_high(MOT_L);     // leve kolo reverz
40
      output_low(MOT_R);      // prave kolo vpred
41
      output_low(MOT_R);      // prave kolo vpred
41
      if(get_timer1()>(odocounter+5))  // konec zatacky?
42
      if(get_timer1()>(odocounter+5))  // konec zatacky?
42
      {
43
      {
43
         disp(0x66);
44
         disp(0x66);
44
         break;
45
         break;
45
      }
46
      }
46
      SetServoQ(cas);
47
      SetServoQ(cas);
47
      delay_ms(18);
48
      delay_ms(18);
48
   };
49
   };
49
 
50
 
50
   //------ Objeti cihly v konstantni vzdalenosti ------
51
   //------ Objeti cihly v konstantni vzdalenosti ------
51
   ridic=pred_carou;
52
   ridic=pred_carou;
52
   cas=CASAVR-CASMIN;   // rovne
53
   cas=CASAVR-CASMIN;   // rovne
53
   output_low(MOT_L);   // vpred
54
   output_low(MOT_L);   // vpred
54
   output_low(MOT_R);
55
   output_low(MOT_R);
55
   while(true)
56
   while(true)
56
   {
57
   {
57
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
58
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
58
      {
59
      {
59
         bum();
60
         bum();
60
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
61
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
61
         goto cihla; // Znovu zacni cihlu objizdet
62
         set_pwm1_duty(140);  // vpred
-
 
63
         set_pwm2_duty(140);
62
         // Pozor! Pamatuje se, jestli jsme uz neprejeli caru!
64
         output_low(MOT_L); 
-
 
65
         output_low(MOT_R);
63
      };
66
      };
64
 
67
 
65
      if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni
68
      if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni
66
      {
69
      {
67
         cas=CASMIN;
70
         cas=CASMIN;
68
      }
71
      }
69
      else
72
      else
70
      {
73
      {
71
         if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
74
         if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
72
         {
75
         {
73
            if(cas>(CASMIN+20)) cas-=20;
76
            if(cas>(CASMIN+20)) cas-=20;
74
         }
77
         }
75
         else
78
         else
76
         {
79
         {
77
            if(cas<(CASMAX-20)) cas+=20;
80
            if(cas<(CASMAX-20)) cas+=20;
78
         };
81
         };
79
      };
82
      };
80
      // Elektronicky diferencial
83
      // Elektronicky diferencial
81
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
84
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
82
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
85
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
83
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
86
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
84
 
87
 
85
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
88
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
86
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
89
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
87
 
90
 
88
      set_pwm1_duty(r1);   // Nastav rychlost motoru
91
      set_pwm1_duty(r1);   // Nastav rychlost motoru
89
      set_pwm2_duty(r2);
92
      set_pwm2_duty(r2);
90
 
93
 
91
      SetServoQ(cas);
94
      SetServoQ(cas);
92
 
95
 
93
      i2c_start();     // Sonar Ping
96
      i2c_start();     // Sonar Ping
94
      i2c_write(SONAR_ADR);
97
      i2c_write(SONAR_ADR);
95
      i2c_write(0x0);
98
      i2c_write(0x0);
96
      i2c_write(0x52);  // mereni v us
99
      i2c_write(0x52);  // mereni v us
97
      i2c_stop();
100
      i2c_stop();
98
 
101
 
99
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
102
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
100
      {
103
      {
101
         set_adc_channel(LMAX);
104
         set_adc_channel(LMAX);
102
         delay_us(100);
105
         delay_us(100);
103
         if(read_adc()<THR) touch|=L_TOUCH;
106
         if(read_adc()<THR) touch|=L_TOUCH;
104
         set_adc_channel(RMAX);
107
         set_adc_channel(RMAX);
105
         delay_us(100);
108
         delay_us(100);
106
         if(read_adc()<THR) touch|=R_TOUCH;
109
         if(read_adc()<THR) touch|=R_TOUCH;
107
      };
110
      };
108
 
111
 
109
      i2c_start();     // Odraz ze sonaru
112
      i2c_start();     // Odraz ze sonaru
110
      i2c_write(SONAR_ADR);
113
      i2c_write(SONAR_ADR);
111
      i2c_write(0x3);
114
      i2c_write(0x3);
112
      i2c_stop();
115
      i2c_stop();
113
      i2c_start();
116
      i2c_start();
114
      i2c_write(SONAR_ADR+1);
117
      i2c_write(SONAR_ADR+1);
115
      vzdalenost=i2c_read(0);
118
      vzdalenost=i2c_read(0);
116
      i2c_stop();
119
      i2c_stop();
117
 
120
 
118
      if(touch==L_TOUCH) disp(0xC0);
121
      if(touch==L_TOUCH) disp(0xC0);
119
      if(touch==R_TOUCH) disp(0x03);
122
      if(touch==R_TOUCH) disp(0x03);
120
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
123
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
121
      if((ridic==na_care)&&(touch==0)) break;
124
      if((ridic==na_care)&&(touch==0)) break;
122
      if(ridic==na_care) touch=0;
125
      if(ridic==na_care) touch=0;
123
   };
126
   };
124
   disp(0xC3);
127
   disp(0xC3);
125
 
128
 
126
   set_pwm1_duty(20);
129
   set_pwm1_duty(20);
127
   set_pwm2_duty(200);
130
   set_pwm2_duty(200);
128
   output_high(MOT_L);
131
   output_high(MOT_L);
129
   output_low(MOT_R);
132
   output_low(MOT_R);
130
   delay_us(40);
133
   delay_us(40);
131
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
134
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
132
   while (true)  // Znovu se musime dotknout cary
135
   while (true)  // Znovu se musime dotknout cary
133
   {
136
   {
134
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
137
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
135
      {
138
      {
136
         set_adc_channel(LMAX);    // Levy UV sensor
139
         set_adc_channel(LMAX);    // Levy UV sensor
137
         delay_us(100);
140
         delay_us(100);
138
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
141
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
139
         {
142
         {
140
            disp(0xE0);
143
            disp(0xE0);
141
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
144
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
142
            goto cara;
145
            goto cara;
143
         };
146
         };
144
         set_adc_channel(RMAX);    // Pravy UV sensor
147
         set_adc_channel(RMAX);    // Pravy UV sensor
145
         delay_us(100);
148
         delay_us(100);
146
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
149
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
147
         {
150
         {
148
            disp(0x07);
151
            disp(0x07);
149
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
152
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
150
            goto cara;
153
            goto cara;
151
         };
154
         };
152
      }
155
      }
153
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
156
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
154
   }
157
   }
155
 
158
 
156
cara:
159
cara:
157
 
160
 
158
   output_low(MOT_L); // oba motory vpred
161
   output_low(MOT_L); // oba motory vpred
159
   output_low(MOT_R);
162
   output_low(MOT_R);