Subversion Repositories svnkaklik

Rev

Rev 222 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 222 Rev 223
Line 2... Line 2...
2 
 
 2 
 
3 
int8 n;
 3 
int8 n;
4 
int8 r1,r2,rr;
 4 
int8 r1,r2,rr;
5 
int1 flag;
 5 
int1 flag;
6 
 
 6 
 
7 
SetServo(CASMIN-3);   // max. doleva, zatoc kolmo na caru
 7 
SetServo(CASMIN-5);   // max. doleva, zatoc kolmo na caru
8 
set_pwm1_duty(0);
 8 
set_pwm1_duty(0);
9 
set_pwm2_duty(140);
 9 
set_pwm2_duty(140);
10 
output_low(MOT_L);    // vpred
 10 
output_low(MOT_L);    // vpred
11 
output_low(MOT_R);
 11 
output_low(MOT_R);
12 
disp(1);
 12 
disp(0x81);
13 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 13 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
14 
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
 14 
while(get_timer1()<(odocounter+6)); // Popojed definovanou vzdalenost
15 
 
 15 
 
16 
set_pwm1_duty(80);
 16 
set_pwm1_duty(80);
17 
set_pwm2_duty(80);
 17 
set_pwm2_duty(80);
18 
set_adc_channel(LMAX);
 18 
set_adc_channel(LMAX);
19 
disp(2);
 19 
disp(0x82);
20 
cas=CASAVR-CASMIN;
 20 
cas=CASAVR-CASMIN;
21 
odocounter=get_timer1();
 21 
odocounter=get_timer1();
22 
flag=1;
 22 
flag=1;
23 
rr=20;
 23 
rr=20;
24 
while(true)
 24 
while(true)
Line 49... Line 49...
49 
 
 49 
 
50 
      while(input(HREF));
 50 
      while(input(HREF));
51 
   };
 51 
   };
52 
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
 52 
   if(get_timer1()>(odocounter+5)) // Prodleva, nez se zacne detekovat cara
53 
   {
 53 
   {
54 
      if(flag==1) {disp(3); flag=0;};
 54 
      if(flag==1) {disp(0x83); flag=0;};
55 
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};};
 55 
      if(read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) {delay_us(600); if (read_adc()<THR_L) break;};}; // Neni to jen skvira nebo spina?
56 
   };
 56 
   };
57 
};
 57 
};
58 
 
 58 
 
59 
disp(4);
 59 
disp(0x84);
60 
while(read_adc()<THR_L);   // Prejed caru
 - 
 
61 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 60 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 61 
SetServo(CASAVR);    // Primy smer
62 
while((read_adc()<THR_L) || (get_timer1()<(odocounter+1))); // Popojed za caru
 62 
while((read_adc()<THR_L) || (get_timer1()<(odocounter+2))); // Popojed za caru
63 
 
 63 
 
64 
SetServo(CASMIN);   // max. doleva                 L
 64 
SetServo(CASMIN);   // max. doleva                 L
65 
set_pwm1_duty(0);
 65 
set_pwm1_duty(20);
66 
set_pwm2_duty(140);
 66 
set_pwm2_duty(170);
- 
 
 67 
output_high(MOT_L);    // levym kolem brzdi
67 
while (read_adc()>THR_L);
 68 
output_low(MOT_R);
68 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 69 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
- 
 
 70 
while (true)  // Znovu se musime dotknout cary
- 
 
 71 
{
- 
 
 72 
   if(read_adc()<THR_L)   // Dotkli jsme se levym senzorem
- 
 
 73 
   {
- 
 
 74 
      while(read_adc()<THR_L); // Prejedeme caru
- 
 
 75 
      cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
- 
 
 76 
      break;
- 
 
 77 
   };
- 
 
 78 
   set_adc_channel(RMAX);    // Pravy UV sensor
- 
 
 79 
   delay_us(40);
69 
while(get_timer1()<(odocounter+3)); // Popojed definovanou vzdalenost
 80 
   if((get_timer1()>=(odocounter+2)) && (read_adc()<THR_L)) // Pravym senzorem nesmime caru prejet!
- 
 
 81 
   {
- 
 
 82 
      cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
- 
 
 83 
      break;
- 
 
 84 
   };
- 
 
 85 
   set_adc_channel(LMAX);    // Levy UV sensor
- 
 
 86 
   delay_us(40);
- 
 
 87 
}
70 
set_pwm1_duty(0);    // vypni motory
 88 
set_pwm1_duty(0);    // vypni motory
71 
set_pwm2_duty(0);
 89 
set_pwm2_duty(0);
72 
/*
 - 
 
73 
set_pwm1_duty(0);    // reverz (zabrzdi)
 - 
 
74 
set_pwm2_duty(0);
 - 
 
75 
output_low(MOT_L);
 - 
 
76 
output_high(MOT_R);
 - 
 
77 
*/
 - 
 
78 
 
 90 
 
79 
SetServo(CASAVR-CASMIN);   // doprostred
 91 
SetServo(CASAVR-CASMIN);   // doprostred
80 
 
 92 
 
81 
disp(5);
 93 
disp(0x85);
82 
set_pwm1_duty(255);    // max. vpred
 94 
set_pwm1_duty(255);    // max. vpred
83 
set_pwm2_duty(255);
 95 
set_pwm2_duty(255);
84 
output_low(MOT_L);
 96 
output_low(MOT_L);
85 
output_low(MOT_R);
 97 
output_low(MOT_R);
86 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 98 
odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
Line 90... Line 102...
90 
   delay_us(40);
 102 
   delay_us(40);
91 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
 103 
   if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
92 
   set_adc_channel(RMAX);    // Pravy UV sensor
 104 
   set_adc_channel(RMAX);    // Pravy UV sensor
93 
   delay_us(40);
 105 
   delay_us(40);
94 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
 106 
   if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
95 
   cas=CASAVR-CASMIN;    // Cara je rovne
 107 
//   cas=CASAVR-CASMIN;    // Cara je rovne
96 
};
 108 
};