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



   i+=8;
};
 
disp(3);
odocounter=get_timer1();
for(n=1;n<=7;n++)
{
   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i-=8;
};
 
disp(4);
odocounter=get_timer1();
for(n=1;n<=7;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++)
{
   while(get_timer1()<(odocounter+n));
   SetServoQ((CASAVR-CASMIN)-i);
   set_pwm1_duty(130-i);
   i-=16;

disp(7);
while(true)
{
   set_adc_channel(LMAX);    // Levy UV sensor
   delay_us(40);
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; };  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {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()<TRESHOLD) 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()<TRESHOLD) 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()>TRESHOLD); 
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()<TRESHOLD) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
   set_adc_channel(RMAX);    // Pravy UV sensor
   delay_us(40);
   if(read_adc()<TRESHOLD) {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 214	Rev 215
Line 45...	Line 45...
45	`i+=8;`	45	`i+=8;`
46	`};`	46	`};`
47		47
48	`disp(3);`	48	`disp(3);`
49	`odocounter=get_timer1();`	49	`odocounter=get_timer1();`
50	`for(n=1;n<=10;n++)`	50	`for(n=1;n<=7;n++)`
51	`{`	51	`{`
52	`while(get_timer1()<(odocounter+n));`	52	`while(get_timer1()<(odocounter+n));`
53	`SetServoQ((CASAVR-CASMIN)-i);`	53	`SetServoQ((CASAVR-CASMIN)-i);`
54	`set_pwm1_duty(130-i);`	54	`set_pwm1_duty(130-i);`
55	`i-=8;`	55	`i-=8;`
56	`};`	56	`};`
57		57
58	`disp(4);`	58	`disp(4);`
59	`odocounter=get_timer1();`	59	`odocounter=get_timer1();`
60	`for(n=1;n<=10;n++)`	60	`for(n=1;n<=7;n++)`
61	`{`	61	`{`
62	`while(get_timer1()<(odocounter+n));`	62	`while(get_timer1()<(odocounter+n));`
63	`SetServoQ((CASAVR-CASMIN)-i);`	63	`SetServoQ((CASAVR-CASMIN)-i);`
64	`set_pwm1_duty(130-i);`	64	`set_pwm1_duty(130-i);`
65	`i+=8;`	65	`i+=8;`
66	`};`	66	`};`
67		67
68	`disp(5);`	68	`disp(5);`
69	`odocounter=get_timer1();`	69	`odocounter=get_timer1();`
-		70	`//!!!!for(n=1;n<=5;n++)`
70	`for(n=1;n<=5;n++)`	71	`for(n=1;n<=4;n++)`
71	`{`	72	`{`
72	`while(get_timer1()<(odocounter+n));`	73	`while(get_timer1()<(odocounter+n));`
73	`SetServoQ((CASAVR-CASMIN)-i);`	74	`SetServoQ((CASAVR-CASMIN)-i);`
74	`set_pwm1_duty(130-i);`	75	`set_pwm1_duty(130-i);`
75	`i-=16;`	76	`i-=16;`
Line 90...	Line 91...
90	`disp(7);`	91	`disp(7);`
91	`while(true)`	92	`while(true)`
92	`{`	93	`{`
93	`set_adc_channel(LMAX); // Levy UV sensor`	94	`set_adc_channel(LMAX); // Levy UV sensor`
94	`delay_us(40);`	95	`delay_us(40);`
95	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	96	`if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; }; // Prejeli jsme caru vlevo`
96	`set_adc_channel(RMAX); // Pravy UV sensor`	97	`set_adc_channel(RMAX); // Pravy UV sensor`
97	`delay_us(40);`	98	`delay_us(40);`
98	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	99	`if(read_adc()<TRESHOLD) {delay_us(600); if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; }; // Prejeli jsme caru vpravo`
99	`}`	100	`}`
100		101
101		102
102	`if(cas==CASMIN)`	103	`if(cas==CASMIN)`
103	`{`	104	`{`
104	`set_adc_channel(RMAX); // Pravy UV sensor`	105	`set_adc_channel(RMAX); // Pravy UV sensor`
105	`for(j=0;j<10000;j++)`	106	`for(j=0;j<10000;j++)`
106	`{`	107	`{`
107	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`	108	`if(input(HREF)) {SetServoQ(CASMIN); while(input(HREF));}; // doleva`
108	`if (read_adc()<128) break;`	109	`if (read_adc()<TRESHOLD) break;`
109	`}`	110	`}`
110	`}`	111	`}`
111	`else`	112	`else`
112	`{`	113	`{`
113	`set_adc_channel(LMAX); // Pravy UV sensor`	114	`set_adc_channel(LMAX); // Levy UV sensor`
114	`for(j=0;j<10000;j++)`	115	`for(j=0;j<10000;j++)`
115	`{`	116	`{`
116	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`	117	`if(input(HREF)) {SetServoQ(CASMAX); while(input(HREF));}; // doprava`
117	`if (read_adc()<128) break;`	118	`if (read_adc()<TRESHOLD) break;`
118	`}`	119	`}`
119	`}`	120	`}`
120		121
121	`set_adc_channel(RMAX); // Pravy UV sensor`	122	`set_adc_channel(RMAX); // Pravy UV sensor`
122		123
Line 132...	Line 133...
132	`SetServo(CASMIN); // max. doleva L`	133	`SetServo(CASMIN); // max. doleva L`
133	`set_pwm1_duty(0); // vzad`	134	`set_pwm1_duty(0); // vzad`
134	`set_pwm2_duty(0);`	135	`set_pwm2_duty(0);`
135	`output_low(MOT_L);`	136	`output_low(MOT_L);`
136	`output_high(MOT_R);`	137	`output_high(MOT_R);`
137	`while (read_adc()>128);`	138	`while (read_adc()>TRESHOLD);`
138	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	139	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
139	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`	140	`while(get_timer1()<(odocounter+4)); // Popojed definovanou vzdalenost`
140	`set_pwm1_duty(0); // reverz (zabrzdi)`	141	`set_pwm1_duty(0); // reverz (zabrzdi)`
141	`set_pwm2_duty(255);`	142	`set_pwm2_duty(255);`
142	`output_low(MOT_L);`	143	`output_low(MOT_L);`
Line 152...	Line 153...
152	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	153	`odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
153	`while(get_timer1()<(odocounter+2)) // Ujed kousek`	154	`while(get_timer1()<(odocounter+2)) // Ujed kousek`
154	`{`	155	`{`
155	`set_adc_channel(LMAX); // Levy UV sensor`	156	`set_adc_channel(LMAX); // Levy UV sensor`
156	`delay_us(40);`	157	`delay_us(40);`
157	`if(read_adc()<128) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`	158	`if(read_adc()<TRESHOLD) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo`
158	`set_adc_channel(RMAX); // Pravy UV sensor`	159	`set_adc_channel(RMAX); // Pravy UV sensor`
159	`delay_us(40);`	160	`delay_us(40);`
160	`if(read_adc()<128) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`	161	`if(read_adc()<TRESHOLD) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo`
161	`cas=CASAVR-CASMIN; // Cara je rovne`	162	`cas=CASAVR-CASMIN; // Cara je rovne`
162	`};`	163	`};`
163		164
164	`//stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`	165	`stav=cihla; // Stav po objeti cihly, uz zadna cihla asi nebude`
165	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`	166	`//odocounter=get_timer1(); // Poznamenej aktualni stav odometrie`
166	`//rr=60; // Nerozumna rychlost pro rozjeti`	167	`//rr=60; // Nerozumna rychlost pro rozjeti`

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/objizdka_R.c @ 215 – Rev 214 → 215