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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_L.c – Rev 235 → 236