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



// **** Objeti cihly vpravo **** RRRR
 
 
int8 n,i;
int16 j;
 
set_adc_channel(RMAX);

  SetServoQ((CASAVR-CASMIN));
};
 
i=0;
 
disp(2);                   // Postupne doleva
set_pwm1_duty(130);
set_pwm2_duty(140);
odocounter=get_timer1();
for(n=1;n<=10;n++)
{

   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i+=8;
};
 
/*
disp(3);
odocounter=get_timer1();
for(n=1;n<=7;n++)
{
   while(get_timer1()<(odocounter+n));

   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i+=8;
};
*/
 
disp(5);
odocounter=get_timer1();
//!!!!for(n=1;n<=5;n++)
for(n=1;n<=4;n++)

disp(7);
while(true)
{
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
   if(read_adc()<THRESHOLD) {delay_us(600); if(read_adc()<THRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<THRESHOLD) {delay_us(600); if(read_adc()<THRESHOLD) {cas=CASMAX; break;}; };  // Prejeli jsme caru vpravo
}
 
 
if(cas==CASMIN)
{
   set_adc_channel(RMAX);    // Pravy UV sensor
   for(j=0;j<10000;j++)
   {
      if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));};  // doleva
      if (read_adc()<THRESHOLD) break;
   }
}
else
{
   set_adc_channel(LMAX);    // Levy UV sensor
   for(j=0;j<10000;j++)
   {
      if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));};  // doprava
      if (read_adc()<THRESHOLD) break;
   }
}
 
set_adc_channel(RMAX);    // Pravy UV sensor
 

SetServo(CASMIN);   // max. doleva                 L
set_pwm1_duty(0);   // vzad
set_pwm2_duty(0);
output_low(MOT_L);
output_high(MOT_R);
while (read_adc()>THRESHOLD);
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost
set_pwm1_duty(0);    // reverz (zabrzdi)
set_pwm2_duty(255);
output_low(MOT_L);

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()<THRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<THRESHOLD) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
   cas=CASAVR-CASMIN;    // Cara je rovne
};
 
//stav=cihla;    // Stav po objeti cihly, uz zadna cihla asi nebude
//odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
//rr=60;   // Nerozumna rychlost pro rozjeti

Rev 217	Rev 221
Line 1...	Line 1...
1	`// ** Objeti cihly vpravo ** RRRR`	1	`// ** Objeti cihly vpravo ** RRRR`
2	`// Verze pouzita na Robot Challenge`	-
3		2
4	`int8 n,i;`	3	`int8 n,i;`
5	`int16 j;`	4	`int16 j;`
6		5
7	`set_adc_channel(RMAX);`	6	`set_adc_channel(RMAX);`
Line 33...	Line 32...
33	`SetServoQ((CASAVR-CASMIN));`	32	`SetServoQ((CASAVR-CASMIN));`
34	`};`	33	`};`
35		34
36	`i=0;`	35	`i=0;`
37		36
38	`disp(2);`	37	`disp(2); // Postupne doleva`
39	`set_pwm1_duty(130);`	38	`set_pwm1_duty(130);`
40	`set_pwm2_duty(140);`	39	`set_pwm2_duty(140);`
41	`odocounter=get_timer1();`	40	`odocounter=get_timer1();`
42	`for(n=1;n<=10;n++)`	41	`for(n=1;n<=10;n++)`
43	`{`	42	`{`
Line 45...	Line 44...
45	`SetServoQ((CASAVR-CASMIN)-i);`	44	`SetServoQ((CASAVR-CASMIN)-i);`
46	`set_pwm1_duty(130-i);`	45	`set_pwm1_duty(130-i);`
47	`i+=8;`	46	`i+=8;`
48	`};`	47	`};`
49		48
-		49	`/*`
50	`disp(3);`	50	`disp(3);`
51	`odocounter=get_timer1();`	51	`odocounter=get_timer1();`
52	`for(n=1;n<=7;n++)`	52	`for(n=1;n<=7;n++)`
53	`{`	53	`{`
54	`while(get_timer1()<(odocounter+n));`	54	`while(get_timer1()<(odocounter+n));`
Line 64...	Line 64...
64	`while(get_timer1()<(odocounter+n));`	64	`while(get_timer1()<(odocounter+n));`
65	`SetServoQ((CASAVR-CASMIN)-i);`	65	`SetServoQ((CASAVR-CASMIN)-i);`
66	`set_pwm1_duty(130-i);`	66	`set_pwm1_duty(130-i);`
67	`i+=8;`	67	`i+=8;`
68	`};`	68	`};`
-		69	`*/`
69		70
70	`disp(5);`	71	`disp(5);`
71	`odocounter=get_timer1();`	72	`odocounter=get_timer1();`
72	`//!!!!for(n=1;n<=5;n++)`	73	`//!!!!for(n=1;n<=5;n++)`
73	`for(n=1;n<=4;n++)`	74	`for(n=1;n<=4;n++)`
Line 93...	Line 94...
93	`disp(7);`	94	`disp(7);`
94	`while(true)`	95	`while(true)`
95	`{`	96	`{`
96	`set_adc_channel(LMAX); // Levy UV sensor`	97	`set_adc_channel(LMAX); // Levy UV sensor`
97	`delay_us(40);`	98	`delay_us(40);`
98	`if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; }; // Prejeli jsme caru vlevo`	99	`if(read_adc()<THRESHOLD) {delay_us(600); if(read_adc()<THRESHOLD) {cas=CASMIN; break;}; }; // Prejeli jsme caru vlevo`
99	`set_adc_channel(RMAX); // Pravy UV sensor`	100	`set_adc_channel(RMAX); // Pravy UV sensor`
100	`delay_us(40);`	101	`delay_us(40);`
101	`if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; }; // Prejeli jsme caru vpravo`	102	`if(read_adc()<THRESHOLD) {delay_us(600); if(read_adc()<THRESHOLD) {cas=CASMAX; break;}; }; // Prejeli jsme caru vpravo`
102	`}`	103	`}`
103		104
104		105
105	`if(cas==CASMIN)`	106	`if(cas==CASMIN)`
106	`{`	107	`{`
107	`set_adc_channel(RMAX); // Pravy UV sensor`	108	`set_adc_channel(RMAX); // Pravy UV sensor`
108	`for(j=0;j<10000;j++)`	109	`for(j=0;j<10000;j++)`
109	`{`	110	`{`
110	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`	111	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`
111	`if (read_adc()<TRESHOLD) break;`	112	`if (read_adc()<THRESHOLD) break;`
112	`}`	113	`}`
113	`}`	114	`}`
114	`else`	115	`else`
115	`{`	116	`{`
116	`set_adc_channel(LMAX); // Levy UV sensor`	117	`set_adc_channel(LMAX); // Levy UV sensor`
117	`for(j=0;j<10000;j++)`	118	`for(j=0;j<10000;j++)`
118	`{`	119	`{`
119	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`	120	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`
120	`if (read_adc()<TRESHOLD) break;`	121	`if (read_adc()<THRESHOLD) break;`
121	`}`	122	`}`
122	`}`	123	`}`
123		124
124	`set_adc_channel(RMAX); // Pravy UV sensor`	125	`set_adc_channel(RMAX); // Pravy UV sensor`
125		126
Line 135...	Line 136...
135	`SetServo(CASMIN); // max. doleva L`	136	`SetServo(CASMIN); // max. doleva L`
136	`set_pwm1_duty(0); // vzad`	137	`set_pwm1_duty(0); // vzad`
137	`set_pwm2_duty(0);`	138	`set_pwm2_duty(0);`
138	`output_low(MOT_L);`	139	`output_low(MOT_L);`
139	`output_high(MOT_R);`	140	`output_high(MOT_R);`
140	`while (read_adc()>TRESHOLD);`	141	`while (read_adc()>THRESHOLD);`
141	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	142	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
142	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	143	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
143	`set_pwm1_duty(0); // reverz (zabrzdi)`	144	`set_pwm1_duty(0); // reverz (zabrzdi)`
144	`set_pwm2_duty(255);`	145	`set_pwm2_duty(255);`
145	`output_low(MOT_L);`	146	`output_low(MOT_L);`
Line 155...	Line 156...
155	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	156	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
156	`while(get_timer1()<(odocounter+2)) // Ujed kousek`	157	`while(get_timer1()<(odocounter+2)) // Ujed kousek`
157	`{`	158	`{`
158	`set_adc_channel(LMAX); // Levy UV sensor`	159	`set_adc_channel(LMAX); // Levy UV sensor`
159	`delay_us(40);`	160	`delay_us(40);`
160	`if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	161	`if(read_adc()<THRESHOLD) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`
161	`set_adc_channel(RMAX); // Pravy UV sensor`	162	`set_adc_channel(RMAX); // Pravy UV sensor`
162	`delay_us(40);`	163	`delay_us(40);`
163	`if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	164	`if(read_adc()<THRESHOLD) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`
164	`cas=CASAVR-CASMIN; // Cara je rovne`	165	`cas=CASAVR-CASMIN; // Cara je rovne`
165	`};`	166	`};`
166		167
167	`stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`	168	`//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`
168	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	169	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
169	`//rr=60; // Nerozumna rychlost pro rozjeti`	170	`//rr=60; // Nerozumna rychlost pro rozjeti`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_R.c – Rev 217 → 221