WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/objizdka_L.c


// **** Objeti cihly vlevo **** LLLL
 
#define L_TOUCH 1    // Cara vlevo
#define R_TOUCH 2    // Cata vpravo
#define B_TOUCH 3    // Both
 
int8 n;
int8 r1,r2,rr;
int8 touch;
enum okolo_cihly {pred_carou,na_care,po_care};
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
int8 vzdalenost;
int8 visualisation;
 
   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
   odocounter=get_timer1();
 
cihla:
 
   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
   disp(0x99);
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
   set_pwm2_duty(255);
   output_high(MOT_L);
   output_low(MOT_R);
 
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
   {
      cas=CASMIN-5;       // jeste vic nez hodne do leva
 
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
      {
         bum();
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
         delay_ms(TUHOS); //!!! Zatuhle prevodovky
         brzda();
         goto cihla; // Znovu zacni cihlu objizdet
      };
 
      set_pwm1_duty(0);
      set_pwm2_duty(220);     // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
      output_high(MOT_L);     // leve kolo reverz
      output_low(MOT_R);      // prave kolo vpred
      if(get_timer1()>(odocounter+5))  // konec zatacky?
      {
         disp(0x66);
         break;
      }
      SetServoQ(cas);
      delay_ms(18);
   };
 
   //------ Objeti cihly v konstantni vzdalenosti ------
   touch=0;    // Indikator detekce cary pri objizdeni
   ridic=pred_carou;
   cas=CASAVR-CASMIN;   // rovne
   output_low(MOT_L);   // vpred
   output_low(MOT_R);
   visualisation=0;
   while(true)
   {
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
      {
         bum();
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
         delay_ms(TUHOS); //!!! Zatuhle prevodovky
         set_pwm1_duty(160);  // vpred
         set_pwm2_duty(160);
         output_low(MOT_L);
         output_low(MOT_R);
         cas=CASMIN;
      };
 
      delta_bearing=bearing-bearing_offset;
      visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F);
      if(IRRX)  // hrozi celni srazka s cihlou v prubehu objizdeni
      {
         cas=CASMIN;
      }
      else
      {
         if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly
         {
            if(cas>(CASMIN+20)) cas-=20;
         }
         else
         {
            if(cas<(CASMAX-20)) cas+=20;
         };
      };
      // Elektronicky diferencial
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 
//!!! pro zatuhle prevodovky
//      r1<<=1;     // Rychlost je dvojnasobna
//      r2<<=1;     // Rozsah 2 az 184 pro rr=0
 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
      set_pwm2_duty(r2);
 
      SetServoQ(cas);
 
      i2c_start();     // Sonar Ping
      i2c_write(SONAR_ADR);
      i2c_write(0x0);
      i2c_write(0x52);  // mereni v us
      i2c_stop();
 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
      {
         set_adc_channel(LMAX);
         delay_us(100);
         if(read_adc()<THR) touch|=L_TOUCH;
         set_adc_channel(RMAX);
         delay_us(100);
         if(read_adc()<THR) touch|=R_TOUCH;
      };
 
      i2c_start();     // Odraz ze sonaru
      i2c_write(SONAR_ADR);
      i2c_write(0x3);
      i2c_stop();
      i2c_start();
      i2c_write(SONAR_ADR+1);
      vzdalenost=i2c_read(0);
      i2c_stop();
 
      i2c_start();     // Cteni kompasu
      i2c_write(COMPAS_ADR);
      i2c_write(0x1);   // uhel 0-255
      i2c_stop();
      i2c_start();
      i2c_write(COMPAS_ADR+1);
      bearing=i2c_read(0);
      i2c_stop();
 
      if(touch==L_TOUCH) visualisation|=0x2;
      if(touch==R_TOUCH) visualisation|=0x1;
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
      if((ridic==na_care)&&(touch==0)) break;
      if(ridic==na_care) touch=0;
      disp(visualisation);
   };
   disp(0xC3);
 
   set_pwm1_duty(20);    //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
   set_pwm2_duty(250);
   output_high(MOT_L);
   output_low(MOT_R);
   delay_us(40);
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
   while (true)  // Znovu se musime dotknout cary
   {
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
      {
         set_adc_channel(LMAX);    // Levy UV sensor
         delay_us(100);
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
         {
            disp(0xE0);
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
            goto cara;
         };
         set_adc_channel(RMAX);    // Pravy UV sensor
         delay_us(100);
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
         {
            disp(0x07);
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
            goto cara;
         };
      }
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
   }
 
cara:
 
   output_low(MOT_L); // oba motory vpred
   output_low(MOT_R);

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);`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_L.c – Rev 255 → 256