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

 //******** Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 191 2007-03-16 21:34:52Z kakl $"
+//"$Id: camerus.c 192 2007-03-16 22:56:25Z kakl $"
 //*****************************************************
 #include ".\camerus.h"
 #USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 #define PIX    PIN_C6      // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator))
 #define SERVO  PIN_B4      // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem))
 #define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
 #define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
 #define MOT_1  PIN_C1      // PWM vystpy motoru
 #define MOT_2  PIN_C2      //
 #define DATA   PIN_B2      // K modulu LEDbar data
 #define CP     PIN_B1      // K modulu LEDbar hodiny
 #define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
 #define IRRX   PIN_B0      // Vstup INT, generuje preruseni pri prekazce
 #define IRTX   PIN_C0      // Modulovani vysilaci IR LED na detekci prekazky
+{
    int8 n,i;
    set_pwm1_duty(0);    // vypni PWM
    set_pwm2_duty(0);
    setup_ccp1(CCP_OFF);
    setup_ccp2(CCP_OFF);
-   for (n=0;n<255;n++)
+   for (n=0;n<150;n++)
+   {
       output_low(MOT_L);
       output_low(MOT_R);
       output_high(MOT_1);
       output_high(MOT_2);
       delay_us(800);
       output_high(MOT_L);
       output_high(MOT_R);
       output_low(MOT_1);
       output_low(MOT_2);
       delay_us(800);
+   }
    output_low(MOT_L); // smer vpred
    output_low(MOT_R);
    setup_ccp1(CCP_PWM); // RC1               // PWM pro motory
    setup_ccp2(CCP_PWM); // RC2
+}
 void SetServo(int8 angle)
       output_low(MOT_R);
       return;
    };
    SetServo((CASAVR-CASMIN));   // rovne
-   delay_ms(100);    // Doba reverzace
+   delay_ms(200);    // Doba reverzace
    brzda();
    if (stav==cihla) while(true); // Zastav na furt, konec drahy
    if(stav==jizda)   // Objed cihlu
+   {
       int n;
       SetServo(CASMIN);   // prudce doleva
-      delay_ms(200);
-      set_pwm1_duty(150);  // vpred
+      set_pwm1_duty(0);  // vpred
       set_pwm2_duty(200);
       output_low(MOT_L);
       output_low(MOT_R);
       n=0;
       while(true)
+      {
          while(input(ODO));
          while(!input(ODO));
          n++;
-         if(n==5) break;
+         if(n==7) break;
+      }
       set_pwm1_duty(0);    // reverz (zabrzdi)
       set_pwm2_duty(0);
-      output_high(MOT_L);
+      output_low(MOT_L);
       output_high(MOT_R);
+      delay_ms(200);
+      brzda();
       SetServo((CASAVR-CASMIN));   // rovne
       set_pwm1_duty(140);  // vpred
       set_pwm2_duty(140);
       output_low(MOT_L);
       while(true)
+      {
          while(input(ODO));
          while(!input(ODO));
          n++;
+         if(n==10) break;
+      }
+      set_pwm1_duty(0);    // reverz (zabrzdi)
+      set_pwm2_duty(0);
+      output_high(MOT_L);
+      output_high(MOT_R);
+      delay_ms(200);
+      brzda();
+      SetServo(CASMIN);   // max. doleva
+      set_pwm1_duty(0);   // vzad
+      set_pwm2_duty(20);
+      output_low(MOT_L);
+      output_high(MOT_R);
+      n=0;
+      while(true)
+      {
+         while(input(ODO));
+         while(!input(ODO));
+         n++;
-         if(n==6) break;
+         if(n==9) break;
+      }
+      set_pwm1_duty(0);    // reverz (zabrzdi)
+      set_pwm2_duty(255);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      delay_ms(200);
       brzda();
-      SetServo((CASAVR-CASMIN)+70);   // doprava
+      SetServo((CASAVR-CASMIN));   // rovne
       set_pwm1_duty(140);  // vpred
       set_pwm2_duty(140);
       output_low(MOT_L);
       output_low(MOT_R);
       n=0;
       while(true)
+      {
          while(input(ODO));
          while(!input(ODO));
          n++;
-         if(n==6) break;
+         if(n==13) break;
+      }
       set_pwm1_duty(0);    // reverz (zabrzdi)
       set_pwm2_duty(0);
       output_high(MOT_L);
       output_high(MOT_R);
-      delay_ms(100);
+      delay_ms(200);
       brzda();
       SetServo(CASMIN);   // max. doleva
       set_pwm1_duty(0);   // vzad
       set_pwm2_duty(20);
       output_low(MOT_L);
       while(true)
+      {
          while(input(ODO));
          while(!input(ODO));
          n++;
-         if(n==5) break;
+         if(n==6) break;
+      }
       set_pwm1_duty(0);    // reverz (zabrzdi)
       set_pwm2_duty(255);
       output_low(MOT_L);
       output_low(MOT_R);
-      delay_ms(100);
+      delay_ms(200);
       brzda();
-      SetServo((CASAVR-CASMIN)+20);   // mirne doprava
+      SetServo((CASAVR-CASMIN));   // rovne
       set_pwm1_duty(190);  // vpred
       set_pwm2_duty(190);
       output_low(MOT_L);
       output_low(MOT_R);
       n=0;
       while(true)
+      {
          while(input(ODO));
          while(!input(ODO));
          n++;
-         if(n==14) break;
+         if(n==9) break;
+      }
 /*
       set_pwm1_duty(0);    // reverz (zabrzdi)
       set_pwm2_duty(0);
       output_high(MOT_L);
       output_high(MOT_R);
       delay_ms(100);
 */
       brzda();
       cas=CASMIN;  // Cara je vlevo
       stav=cihla;
+   }
+}

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 255 – Rev 191 → 192