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



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(255);
   output_high(MOT_L);
   output_low(MOT_R);
   odocounter=get_timer1();
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
   {

 
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
      {
         bum();
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
         delay_ms(TUHOS); //!!! Zatuhle prevodovky
         brzda();
         goto cihla; // Znovu zacni cihlu objizdet
      };
 
      set_pwm1_duty(0);
      set_pwm2_duty(255);     // !!! 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);

   {
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
      {
         bum();
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
         delay_ms(TUHOS); //!!! Zatuhle prevodovky
         set_pwm1_duty(160);  // vpred
         set_pwm2_duty(160);
         output_low(MOT_L);
         output_low(MOT_R);
         cas=CASMIN;

      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);
 
//!!! pro zatuhle prevodovky
      r1<<=1;     // Rychlost je dvojnasobna
      r2<<=1;     // Rozsah 2 az 184 pro rr=0
 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
      set_pwm2_duty(r2);
 
      SetServoQ(cas);
 

      if(ridic==na_care) touch=0;
      disp(visualisation);
   };
   disp(0xC3);
 
   set_pwm1_duty(0);    //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
   set_pwm2_duty(255);
   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

Rev 249	Rev 252
Line 18...	Line 18...
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(255);`
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	`{`
Line 30...	Line 30...
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	`delay_ms(TUHOS); //!!! Zatuhle prevodovky`
35	`brzda();`	36	`brzda();`
36	`goto cihla; // Znovu zacni cihlu objizdet`	37	`goto cihla; // Znovu zacni cihlu objizdet`
37	`};`	38	`};`
38		39
39	`set_pwm1_duty(0);`	40	`set_pwm1_duty(0);`
40	`set_pwm2_duty(220); // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku`	41	`set_pwm2_duty(255); // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku`
41	`output_high(MOT_L); // leve kolo reverz`	42	`output_high(MOT_L); // leve kolo reverz`
42	`output_low(MOT_R); // prave kolo vpred`	43	`output_low(MOT_R); // prave kolo vpred`
43	`if(get_timer1()>(odocounter+5)) // konec zatacky?`	44	`if(get_timer1()>(odocounter+5)) // konec zatacky?`
44	`{`	45	`{`
45	`disp(0x66);`	46	`disp(0x66);`
Line 59...	Line 60...
59	`{`	60	`{`
60	`if(BUMPER) // Narazili jsme do cihly, musime couvnout!`	61	`if(BUMPER) // Narazili jsme do cihly, musime couvnout!`
61	`{`	62	`{`
62	`bum();`	63	`bum();`
63	`SaveLog(log-1); // Zapis Black Boxu do EEPROM`	64	`SaveLog(log-1); // Zapis Black Boxu do EEPROM`
-		65	`delay_ms(TUHOS); //!!! Zatuhle prevodovky`
64	`set_pwm1_duty(160); // vpred`	66	`set_pwm1_duty(160); // vpred`
65	`set_pwm2_duty(160);`	67	`set_pwm2_duty(160);`
66	`output_low(MOT_L);`	68	`output_low(MOT_L);`
67	`output_low(MOT_R);`	69	`output_low(MOT_R);`
68	`cas=CASMIN;`	70	`cas=CASMIN;`
Line 91...	Line 93...
91	`if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92`	93	`if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92`
92		94
93	`if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni`	95	`if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni`
94	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);`	96	`if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);`
95		97
-		98	`//!!! pro zatuhle prevodovky`
-		99	`r1<<=1; // Rychlost je dvojnasobna`
-		100	`r2<<=1; // Rozsah 2 az 184 pro rr=0`
-		101
96	`set_pwm1_duty(r1); // Nastav rychlost motoru`	102	`set_pwm1_duty(r1); // Nastav rychlost motoru`
97	`set_pwm2_duty(r2);`	103	`set_pwm2_duty(r2);`
98		104
99	`SetServoQ(cas);`	105	`SetServoQ(cas);`
100		106
Line 139...	Line 145...
139	`if(ridic==na_care) touch=0;`	145	`if(ridic==na_care) touch=0;`
140	`disp(visualisation);`	146	`disp(visualisation);`
141	`};`	147	`};`
142	`disp(0xC3);`	148	`disp(0xC3);`
143		149
144	`set_pwm1_duty(20);`	150	`set_pwm1_duty(0); //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200`
145	`set_pwm2_duty(200);`	151	`set_pwm2_duty(255);`
146	`output_high(MOT_L);`	152	`output_high(MOT_L);`
147	`output_low(MOT_R);`	153	`output_low(MOT_R);`
148	`delay_us(40);`	154	`delay_us(40);`
149	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	155	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
150	`while (true) // Znovu se musime dotknout cary`	156	`while (true) // Znovu se musime dotknout cary`

Subversion Repositories svnkaklik

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