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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_L.c @ 253 – Rev 246 → 247