Subversion Repositories svnkaklik

Rev

Rev 195 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log

Rev 195 Rev 196
Line 1... Line 1...
1 
//******** Robot Camerus pro IstRobot 2007 ************
 1 
//******** Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 195 2007-03-17 15:56:26Z kakl $"
 2 
//"$Id: camerus.c 196 2007-03-17 19:38:00Z kakl $"
3 
//*****************************************************
 3 
//*****************************************************
4 
 
 4 
 
5 
#include ".\camerus.h"
 5 
#include ".\camerus.h"
6 
 
 6 
 
7 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 7 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
Line 52... Line 52...
52 
 
 52 
 
53 
   set_pwm1_duty(0);    // vypni PWM
 53 
   set_pwm1_duty(0);    // vypni PWM
54 
   set_pwm2_duty(0);
 54 
   set_pwm2_duty(0);
55 
   setup_ccp1(CCP_OFF);
 55 
   setup_ccp1(CCP_OFF);
56 
   setup_ccp2(CCP_OFF);
 56 
   setup_ccp2(CCP_OFF);
57 
   for (n=0;n<150;n++)
 57 
   for (n=0;n<100;n++)
58 
   {
 58 
   {
59 
      output_low(MOT_L);
 59 
      output_low(MOT_L);
60 
      output_low(MOT_R);
 60 
      output_low(MOT_R);
61 
      output_high(MOT_1);
 61 
      output_high(MOT_1);
62 
      output_high(MOT_2);
 62 
      output_high(MOT_2);
Line 67... Line 67...
67 
      output_low(MOT_2);
 67 
      output_low(MOT_2);
68 
      delay_us(800);
 68 
      delay_us(800);
69 
   }
 69 
   }
70 
   output_low(MOT_L); // smer vpred
 70 
   output_low(MOT_L); // smer vpred
71 
   output_low(MOT_R);
 71 
   output_low(MOT_R);
72 
   setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
 72 
   setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
73 
   setup_ccp2(CCP_PWM); // RC2
 73 
   setup_ccp2(CCP_PWM); // RC2
74 
}
 74 
}
75 
 
 75 
 
76 
void SetServo(int8 angle)
 76 
void SetServo(int8 angle)
77 
{
 77 
{
78 
   int8 n, offset;
 78 
   int8 n;
79 
 
 79 
 
80 
   for(n=0; n<14; n++)
 80 
   for(n=0; n<14; n++)
81 
   {
 81 
   {
82 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
 82 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
83 
      delay_us(1000);
 83 
      delay_us(1000);
Line 119... Line 119...
119 
   };
 119 
   };
120 
 
 120 
 
121 
   set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
 121 
   set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
122 
   SetServo((CASAVR-CASMIN));   // rovne
 122 
   SetServo((CASAVR-CASMIN));   // rovne
123 
 
 123 
 
124 
   set_pwm1_duty(100);  // vpred
 124 
   set_pwm1_duty(140);  // vpred
125 
   set_pwm2_duty(100);
 125 
   set_pwm2_duty(140);
126 
   output_low(MOT_L);
 126 
   output_low(MOT_L);
127 
   output_low(MOT_R);
 127 
   output_low(MOT_R);
128 
   i=0;
 128 
   i=0;
129 
   while(true)
 129 
   while(true)
130 
   {
 130 
   {
131 
     while(input(ODO)) if(read_adc()<128) goto brzdi; // Je cihla blizko?
 131 
     while(input(ODO)) if(read_adc()<128) goto brzdi; // Je cihla blizko?
132 
     while(!input(ODO));
 132 
     while(!input(ODO));
133 
     i++;
 133 
     i++;
134 
     if(i==8) return; // nedojeli jsme k cihle, jed dal
 134 
     if(i==7) return; // nedojeli jsme k cihle, jed dal
135 
   };
 135 
   };
- 
 
 136 
 
136 
brzdi:
 137 
brzdi:
137 
   set_pwm1_duty(0);    // reverz (zabrzdi)
 138 
   set_pwm1_duty(0);    // reverz (zabrzdi)
138 
   set_pwm2_duty(0);
 139 
   set_pwm2_duty(0);
139 
   output_high(MOT_L);
 140 
   output_high(MOT_L);
140 
   output_high(MOT_R);
 141 
   output_high(MOT_R);
141 
   delay_ms(100);
 142 
   delay_ms(150);
142 
   brzda();
 143 
   brzda();
143 
 
 144 
 
144 
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
 145 
   if (stav==cihla) while(true); // Zastav na furt, konec drahy
145 
 
 146 
 
146 
   if(stav==jizda)   // Objed cihlu
 147 
   if(stav==jizda)   // Objed cihlu
Line 408... Line 409...
408 
            disp(0x80);
 409 
            disp(0x80);
409 
            while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
 410 
            while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
410 
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
 411 
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
411 
            set_pwm2_duty(255);
 412 
            set_pwm2_duty(255);
412 
            disp(0x1);
 413 
            disp(0x1);
413 
            delay_ms(400);
 414 
            delay_ms(300);
414 
            stav=jizda;
 415 
            stav=jizda;
415 
         };
 416 
         };
416 
      }
 417 
      }
417 
 
 418 
 
418 
      pom=0x80;    // Zobrazeni pozice cary na displayi
 419 
      pom=0x80;    // Zobrazeni pozice cary na displayi