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



   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?

         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(0x52);  // mereni v us
      i2c_stop();
 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
      {
         set_adc_channel(LMAX);

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

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

Rev 228	Rev 229
Line 21...	Line 21...
21	`output_low(MOT_R);`	21	`output_low(MOT_R);`
22	`odocounter=get_timer1();`	22	`odocounter=get_timer1();`
23	`while(true) // Na zacatku se vyhni cihle, zatoc co muzes`	23	`while(true) // Na zacatku se vyhni cihle, zatoc co muzes`
24	`{`	24	`{`
25	`cas=CASMIN-5; // jeste vic nez hodne do leva`	25	`cas=CASMIN-5; // jeste vic nez hodne do leva`
26		26
27	`set_pwm1_duty(0);`	27	`set_pwm1_duty(0);`
28	`set_pwm2_duty(200);`	28	`set_pwm2_duty(200);`
29	`output_high(MOT_L); // leve kolo reverz`	29	`output_high(MOT_L); // leve kolo reverz`
30	`output_low(MOT_R); // prave kolo vpred`	30	`output_low(MOT_R); // prave kolo vpred`
31	`if(get_timer1()>(odocounter+5)) // konec zatacky?`	31	`if(get_timer1()>(odocounter+5)) // konec zatacky?`
Line 34...	Line 34...
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	`cas=CASAVR-CASMIN; // rovne`
44	`output_low(MOT_L); // vpred`	44	`output_low(MOT_L); // vpred`
45	`output_low(MOT_R);`	45	`output_low(MOT_R);`
46	`while(true)`	46	`while(true)`
47	`{`	47	`{`
48	`// if(!input(IRRX)) goto cihla;`	48	`// if(!input(IRRX)) goto cihla;`
49		49
50	`if((vzdalenost!=0)\|\|!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly`	50	`if((vzdalenost!=0)\|\|!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly`
51	`{`	51	`{`
52	`if(cas>(CASMIN+20)) cas-=20;`	52	`if(cas>(CASMIN+20)) cas-=20;`
53	`}`	53	`}`
54	`else`	54	`else`
55	`{`	55	`{`
56	`if(cas<(CASMAX-20)) cas+=20;`	56	`if(cas<(CASMAX-20)) cas+=20;`
57	`};`	57	`};`
58		58
59	`// Elektronicky diferencial`	59	`// Elektronicky diferencial`
60	`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`
61	`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`
62	`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`
63		63
64	`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`
65	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);`	65	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);`
66		66
67	`set_pwm1_duty(r1); // Nastav rychlost motoru`	67	`set_pwm1_duty(r1); // Nastav rychlost motoru`
68	`set_pwm2_duty(r2);`	68	`set_pwm2_duty(r2);`
69		69
70	`SetServoQ(cas);`	70	`SetServoQ(cas);`
71		71
72	`i2c_start(); // Sonar Ping`	72	`i2c_start(); // Sonar Ping`
73	`i2c_write(0xE0);`	73	`i2c_write(0xE0);`
74	`i2c_write(0x0);`	74	`i2c_write(0x0);`
75	`i2c_write(0x51);`	75	`i2c_write(0x52); // mereni v us`
76	`i2c_stop();`	76	`i2c_stop();`
77		77
78	`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`
79	`{`	79	`{`
80	`set_adc_channel(LMAX);`	80	`set_adc_channel(LMAX);`
Line 85...	Line 85...
85	`if(read_adc()<THR) touch\|=R_TOUCH;`	85	`if(read_adc()<THR) touch\|=R_TOUCH;`
86	`};`	86	`};`
87		87
88	`i2c_start(); // Odraz ze sonaru`	88	`i2c_start(); // Odraz ze sonaru`
89	`i2c_write(0xE0);`	89	`i2c_write(0xE0);`
90	`i2c_write(0x3);`	90	`i2c_write(0x3);`
91	`i2c_stop();`	91	`i2c_stop();`
92	`i2c_start();`	92	`i2c_start();`
93	`i2c_write(0xE1);`	93	`i2c_write(0xE1);`
94	`vzdalenost=i2c_read(0);`	94	`vzdalenost=i2c_read(0);`
95	`i2c_stop();`	95	`i2c_stop();`
96		96
97	`if(touch==L_TOUCH) disp(0xC0);`	97	`if(touch==L_TOUCH) disp(0xC0);`
98	`if(touch==R_TOUCH) disp(0x03);`	98	`if(touch==R_TOUCH) disp(0x03);`
99	`if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;`	99	`if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;`
100	`if((ridic==na_care)&&(touch==0)) break;`	100	`if((ridic==na_care)&&(touch==0)) break;`
101	`if(ridic==na_care) touch=0;`	101	`if(ridic==na_care) touch=0;`
102	`};`	102	`};`
103	`disp(0xC3);`	103	`disp(0xC3);`
104		104
105	`set_pwm1_duty(20);`	105	`set_pwm1_duty(20);`
106	`set_pwm2_duty(200);`	106	`set_pwm2_duty(200);`
107	`output_high(MOT_L);`	107	`output_high(MOT_L);`
108	`output_low(MOT_R);`	108	`output_low(MOT_R);`
109	`delay_us(40);`	109	`delay_us(40);`
Line 129...	Line 129...
129	`goto cara;`	129	`goto cara;`
130	`};`	130	`};`
131	`}`	131	`}`
132	`SetServoQ(CASMIN-5); // max. max. doleva L`	132	`SetServoQ(CASMIN-5); // max. max. doleva L`
133	`}`	133	`}`
134		134
135	`cara:`	135	`cara:`
136		136
137	`output_low(MOT_L); // oba motory vpred`	137	`output_low(MOT_L); // oba motory vpred`
138	`output_low(MOT_R);`	138	`output_low(MOT_R);`
139	`/*`	139	`/*`
140	`set_pwm1_duty(0); // vypni motory`	140	`set_pwm1_duty(0); // vypni motory`
141	`set_pwm2_duty(0);`	141	`set_pwm2_duty(0);`

Subversion Repositories svnkaklik

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