Detekce překážky a změna směru před překážkou

#include <Motors.h>
#include <Measure_distance.h>
#include <MuxIR.h>

#define MIN_VZ 10
#define MD 200  //bezpecnostni delay, způsobuje stoceni

Motors motory(0,12);

Measure_distance dalkomer_C(10);
Measure_distance dalkomer_Z(11);
Measure_distance dalkomer_L(9);
Measure_distance dalkomer_P(8);

int smer;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  jed();
  smer=1;
}


void loop() {
  // put your main code here, to run repeatedly:
  if(smer==1)
  {
    //jede dopredu
    if(dalkomer_C.take_measure()<MIN_VZ)
    {
        stop();
        couvej();
        smer=-1;
    }
  }
  else
  {
    //couva
    if(dalkomer_Z.take_measure()<MIN_VZ)
    {
       stop();
        jed();
        smer=1;
    }
  }

  delay(20);

/*
 vypis_stav();
 delay(2000);
  */
}
Poslední úprava stránky: 25.11.2019, 10:54
Powered by PmWiki