Subversion Repositories svnkaklik

Rev

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

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