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;
 
cihla:
 
   rr=20;
   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 ------
   touch=0;
   ridic=pred_carou;
   cas=CASAVR-CASMIN;   // rovne
   output_low(MOT_L);   // vpred
   output_low(MOT_R);
   while(true)
   {
//      if(!input(IRRX)) goto cihla;
   
      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(0xE0);
      i2c_write(0x0);  
      i2c_write(0x51);
      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(0xE0);
      i2c_write(0x3);  
      i2c_stop();
      i2c_start();     
      i2c_write(0xE1);
      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);
/*
set_pwm1_duty(0);    // vypni motory
set_pwm2_duty(0);
 
SetServo(CASAVR-CASMIN);   // doprostred
 
set_pwm1_duty(255);    // max. vpred
set_pwm2_duty(255);
output_low(MOT_L);
output_low(MOT_R);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+2)) // Ujed kousek
{
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
   if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
};
*/

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

Subversion Repositories svnkaklik

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