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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_L.c – Rev 248 → 249