forum.hwkitchen.cz

Kód: Vybrat vše

//******************************************** KMITANI SERVA  ************************************************************************************************************

void  zmerit_vzdalenost() {                   //čtení vzdálenosti ultrazvuku    "vpred = vzdálenost naměřená ultrazvukem"
      
    sonarServo.write (70,50,true);           // vzdálenost mírně vlevo 70°    50 - rychlost otáčení
    delay (10);
    ultrazvuk ();
     vzdalenost = vpred;

    sonarServo.write (110,50,true);          // vzdálenost mírně vpravo 110°
    delay (10);
    ultrazvuk ();
    if (vpred <= vzdalenost ) { vzdalenost = vpred; }

    sonarServo.write (90,50,true);            // vzdálenost přímo vpřed 90°
    delay (10);
    ultrazvuk ();
    if (vpred <= vzdalenost ) { vzdalenost = vpred; }
     OLED ();

Kód: Vybrat vše

//----------------------------------   POHYB   PODLE VZDALENOSTI   ---------------------------------------------------------------------------------------

   zmerit_vzdalenost();           // kmitání serva sonaru
   sonarServo.stop ();            // ukončit kmitání serva
   //ultrazvuk ();
   //vzdalenost = vpred;
   
      if (vzdalenost <= 30) {
          Controller.stopActionGroup();
          Controller.runActionGroup(GO_BACK, 2);     // dva kroky zpět
          delay (2500);
            sonar1 ();
            //goto konec;  
      }
      
      if ((vzdalenost >= 80 ) and (xxxB == 1)) {     // vzd. větší než 100 a běží pomalu
        Controller.stopActionGroup();                 // stop
        xxxAA = 0;                                    // pomocná proměnná
      }
            
      if ((vzdalenost >= 80 ) and (xxxAA == 0)){             // vzd. větší než 100 a stojí
          Controller.runActionGroup(RYCHLE_DLOUHY_KROK, 0);   // stále běžet rychle 
          xxxA = 1;                                           // běží rychle
          xxxB = 0;
      }
      
      if ((vzdalenost < 80) and (xxxA == 1)){              // vzd. menší než 100 a běží rychle          
          Controller.stopActionGroup();                     // stop
          xxxBB = 0;                                         // 
      }

      if ((vzdalenost < 80) and (xxxBB == 0)){             // vzd. menší než 100 a stojí
          Controller.runActionGroup(POMALU_POKUS, 0);       // stále běžet pomalu
          xxxB = 1;
          xxxA = 0;
      }  

Kód: Vybrat vše

//*************************************   GYROSKOP    *************************************************************************************************

void gyroskop() {
    
  // zjistí všechny hodnoty z akcelerometru    accelgyro.getMotion (&ax, &ay, &az);   accelgyro.getAcceleration(&ax, &ay, &az);
  //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  counter = 0;
  while (counter < 100)             // provede  měření
  {
    accelgyro.getAcceleration(&ax, &ay, &az);
  
  ax_p = ax_p + ax;
  ay_p = ay_p + ay;
  az_p = az_p + az;

  counter++;
  }
    
    x = ax_p/counter;               //zjistíme průmerné hodnoty
    y = ay_p/counter;
    z = az_p/counter;
    
    angle_x = abs(atan2(x, sqrt(pow(y,2) + pow(z,2)))/(pi/180));
    angle_y = abs(atan2(y, sqrt(pow(x,2) + pow(z,2)))/(pi/180));    //angle_z = atan2(az, sqrt(square(ax) + square(ay)))/(pi/180);
    angle_z = abs(atan2(z, sqrt(pow(x,2) + pow(y,2)))/(pi/180));
    
    //counter = 0;
    ax_p = 0;
    ay_p = 0;
    az_p = 0;
  }

Kód: Vybrat vše

    int zChyba = 88;
    
    x = ax_p/counter;               //zjistíme průmerné hodnoty
    y = ay_p/counter;
    z = az_p/counter;
    z = z + zChyba;			//počítáme s chybou