hindernis erkennung omni-move

hallo allerseits,
ich versuche nicht nur den omni-move zum leben zu erwecken, sondern ihm auch respekt vor hindernissen beizubringen :-), mit mehr oder weniger erfolg. Er:

- fährt nach dem drücken der starttaste los
- losfährt nur dann, wenn kein hindernis in fahrtrichtung vorhanden ist
- dreht sich nach links, wenn ein hindernis beim start detektiert wird

- er detektiert aber keine hindernisse, während er fährt
- kommt aus dem drehen nicht mehr raus, obwohl eine drehung von nur 5° vorgegeben ist

Wahrscheinlich liegt es am setzen der variablen "hindernis" mit false, bzw. true und die damit verbundene reaktion zusammen. Inzwischen sehe ich aber vor lauter bäumen den wald nicht und find den fehler nicht. Könnte bitte jemand über den code drüber schauen?

Code:

---

#include <CustomStepper.h>
#include <NewPing.h>
#include <LCD.h>
#include <LiquidCrystal_I2C.h>
#include <Bounce2.h>
#include <Wire.h>

#define TRIGGER_PIN  6
#define ECHO_PIN    7
#define MAX_DISTANCE 400

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

uint8_t idx;
uint16_t distanz;
uint16_t uS;

enum stepper_e
{ stepper_HM, stepper_VR, stepper_VL, stepper_MAX };


CustomStepper stepper[stepper_MAX]
{
  CustomStepper(35, 37, 39, 41),//stepper_HM-I
  CustomStepper(23, 25, 27, 29),//stepper_VR-II
  CustomStepper(47, 49, 51, 53) //stepper_VL-III
};

boolean start_ping;
boolean hindernis;
boolean start;


Bounce  pin5  = Bounce();


void setup()
{
  pinMode(5, INPUT_PULLUP);
  pin5.attach(5);
  pin5.interval(5);


  start_ping = false;
  hindernis = false;
  start = false;


  Serial.begin(115200);
  Serial1.begin(115200);
  lcd.begin(16, 2);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.setBacklight(HIGH);
  lcd.print("cruise_av_col_2");
  lcd.setCursor(0, 1);
  lcd.print("start_taster");
  delay(2000);
  lcd.clear();
  lcd.setBacklight(LOW);

}

void loop()
{

  if (pin5.update())
  {
    if (pin5.fell())
    {
      start = !start;
    }
  }

  if (start)
  {

    //    fahrt_1();

    hindernis_vorh();

    if (hindernis == false)
    {
      vorwaerts();
      Serial1.println("fahre richtung HM");
      fahrt_ausfuehren();
    }
    else
    {
      rotate_left_deg();
      fahrt_ausfuehren();
    }
  }
}
/***********************************************************/

void hindernis_vorh(void)
{
  if (start_ping == false) ping_distanz();
  if (uS != NO_ECHO)

  {
    if (((uS / US_ROUNDTRIP_CM) <= 25))
      hindernis = true;
  } else
  {
    hindernis = false;
  }
}


void ping_distanz(void)
{
  uS = sonar.ping();
  Serial1.print("Ping: ");
  Serial1.print(uS / US_ROUNDTRIP_CM);  Serial1.println(" cm");
  Serial.print("Ping: ");
  Serial.print(uS / US_ROUNDTRIP_CM);  Serial.println(" cm");
  start_ping = true;
}


void alle_stepper_stop()
{
  for (idx = stepper_HM; idx < stepper_MAX; idx++)
  {
    stepper[idx].setRPM(0);
    stepper[idx].setSPR(4075.7728395);
    stepper[idx].setDirection(STOP);
  }
}


void vorwaerts()
{
  if (start_ping == true)  ping_distanz();

  if (hindernis == true)
  {
    /*
        Serial1.print(hindernis);
        Serial1.println("  hindernis - true - rotiere links- ");
        Serial.print(hindernis);
        Serial.println("  hindernis - true - rotiere links- ");
    */
    rotate_left_deg();
    fahrt_ausfuehren();
    hindernis = false;
  }
  else

    hindernis = false;
  /*
    Serial.print(hindernis);
    Serial1.println("  freie fahrt - richtung HM");
    Serial.print(hindernis);
    Serial.println("  freie fahrt - richtung HM");
  */
  richtung_HM();
  fahrt_ausfuehren();

}

void richtung_HM()
{

  if (start_ping == true)  ping_distanz();

  if (hindernis == true)
  {
    /*
        Serial1.print(hindernis);
        Serial1.println("  hindernis - true - rotiere links- ");
        Serial.print(hindernis);
        Serial.println("  hindernis - true - rotiere links- ");
    */

    rotate_left_deg();
    fahrt_ausfuehren();
    hindernis = false;
  }
  else
  {
    hindernis == false;
    stepper[stepper_HM].setRPM(12);
    stepper[stepper_VR].setRPM(12);
    stepper[stepper_VL].setRPM(12);

    stepper[stepper_HM].setSPR(4075.7728395);
    stepper[stepper_VR].setSPR(4075.7728395);
    stepper[stepper_VL].setSPR(4075.7728395);

    stepper[stepper_VL].setDirection(CCW);
    stepper[stepper_VL].rotate(2);

    stepper[stepper_VR].setDirection(CW);
    stepper[stepper_VR].rotate(2);

  }

}


void richtung_VL()
{
  stepper[stepper_HM].setRPM(12);
  stepper[stepper_VR].setRPM(12);
  stepper[stepper_VL].setRPM(12);

  stepper[stepper_HM].setSPR(4075.7728395);
  stepper[stepper_VR].setSPR(4075.7728395);
  stepper[stepper_VL].setSPR(4075.7728395);

  stepper[stepper_VR].setDirection(CCW);
  stepper[stepper_VR].rotate(2);

  stepper[stepper_HM].setDirection(CW);
  stepper[stepper_HM].rotate(2);
}


void richtung_VR()
{
  stepper[stepper_HM].setRPM(12);
  stepper[stepper_VR].setRPM(12);
  stepper[stepper_VL].setRPM(12);

  stepper[stepper_HM].setSPR(4075.7728395);
  stepper[stepper_VR].setSPR(4075.7728395);
  stepper[stepper_VL].setSPR(4075.7728395);

  stepper[stepper_VL].setDirection(CW);
  stepper[stepper_VL].rotate(2);

  stepper[stepper_HM].setDirection(CCW);
  stepper[stepper_HM].rotate(2);
}


void test_richtung() //VR
{
  stepper[stepper_HM].setRPM(12);
  stepper[stepper_VR].setRPM(12);
  stepper[stepper_VL].setRPM(12);

  stepper[stepper_HM].setSPR(4075.7728395);
  stepper[stepper_VR].setSPR(4075.7728395);
  stepper[stepper_VL].setSPR(4075.7728395);

  stepper[stepper_VL].setDirection(CW);
  stepper[stepper_VL].rotate(2);

  stepper[stepper_HM].setDirection(CCW);
  stepper[stepper_HM].rotate(2);
}


void rotate_right_deg(void)
{
  for (idx = stepper_HM; idx < stepper_MAX; idx++)
  {
    stepper[idx].setRPM(12);
    stepper[idx].setSPR(4075.7728395);
    stepper[idx].setDirection(CW);
    stepper[idx].rotateDegrees(60);
  }
}


void rotate_left_deg(void)
{

  hindernis = false;
  for (idx = stepper_HM; idx < stepper_MAX; idx++)
  {

    stepper[idx].setRPM(12);
    stepper[idx].setSPR(4075.7728395);
    stepper[idx].setDirection(CW);
    stepper[idx].rotateDegrees(5);
  }

}


void rotate_right(void)
{
  for (idx = stepper_HM; idx < stepper_MAX; idx++)
  {
    stepper[idx].setRPM(12);
    stepper[idx].setSPR(4075.7728395);
    stepper[idx].setDirection(CCW);
    stepper[idx].rotate(2);
  }
}


void rotate_left(void)
{

  for (idx = stepper_HM; idx < stepper_MAX; idx++)
  {
    stepper[idx].setRPM(12);
    stepper[idx].setSPR(4075.7728395);
    stepper[idx].setDirection(CW);
    stepper[idx].rotate(2);
  }
}


boolean fahrt_fertig()
{
  return        stepper[stepper_HM].isDone() && stepper[stepper_VR].isDone()
                && stepper[stepper_VL].isDone();
}

void fahrt_ausfuehren()
{
  while ( ! fahrt_fertig() )
  {
    for (idx = stepper_HM; idx < stepper_MAX; idx++)
    {
      stepper[idx].run();
      // delay(1);
    }
  }
}

/************************************************************/

void fahrt_1()
{
  rotate_right();
  Serial1.println("drehe rechts");
  fahrt_ausfuehren();

  alle_stepper_stop();
  fahrt_ausfuehren();

  richtung_HM();
  Serial1.println("fahre richtung HM");
  fahrt_ausfuehren();

  alle_stepper_stop();
  fahrt_ausfuehren();

  richtung_VR();
  fahrt_ausfuehren();

  alle_stepper_stop();
  fahrt_ausfuehren();

  richtung_VL();
  fahrt_ausfuehren();

  alle_stepper_stop();
  fahrt_ausfuehren();

  rotate_left();
  fahrt_ausfuehren();

  alle_stepper_stop();
  fahrt_ausfuehren();
}

void halt()
{
  alle_stepper_stop();
  fahrt_ausfuehren();
}

---

vielen dank...

Hallo,

ich habe ein wenig diese Funktion in Verdacht

Code:

---

void hindernis_vorh(void)
{
  if (start_ping == false) ping_distanz();
  if (uS != NO_ECHO)

  {
    if (((uS / US_ROUNDTRIP_CM) <= 25))
      hindernis = true;
  } else
  {
    hindernis = false;
  }
}

---

Wenn der Wert kleiner gleich 25 ist, wird Hindernis auf true gesetzt. Dann muss der Wert auf NO_ECHO zurückgehen, um wieder auf false gesetzt zu werden.

Soll das wirklich so sein ?

diese funktion habe ich bereits in diesem thread zum 4-rad robby verwendet, dort funktioniert sie einwandfrei...

Ah, in rotate_left_deg wird hindernis auf false gesetzt. Ok, dann ist es das nicht.

Aber wenn er mit dem Drehen nicht aufhört, bedeutet das dann, dass er aus dieser Funktion nicht wieder rausgeht ? Eventuell steckt er in der for-Schleife ?

Hast du Möglichkeiten zur Ausgabe ? Kannst du notfalls erstmal am Anfang von loop eine LED blinken lassen und dann erst den Rest machen ?
Wenn er nur einen loop Durchlauf macht, dann in rotate_left_deg schauen, ob er aus der Schleife wieder raus geht.

ich habe die funktion "rotate _left_deg" um printausgabe ergänzt:

Code:

---

void rotate_left_deg(void)
{

  hindernis = false;
  Serial1.println("  hindernis - true - rotiere links- ");
  Serial.println("  hindernis - true - rotiere links- ");

  for (idx = stepper_HM; idx < stepper_MAX; idx++)
  {

    stepper[idx].setRPM(12);
    stepper[idx].setSPR(4075.7728395);
    stepper[idx].setDirection(CW);
    stepper[idx].rotateDegrees(5);
  }

}

---

die ausgabe aus der drehfunktion kommt dauernd:

Code:

---

Ping: 10 cm
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links- 
  hindernis - true - rotiere links-

---

es liegt m.e. nach nicht an der for scheife...

Hm,

verschiebe die Ausgabe bitte mal in das loop() und zwar so

Code:

---

  // ...
    if (hindernis == false)
    {
      vorwaerts();
      Serial1.println("fahre richtung HM");
      fahrt_ausfuehren();
    }
    else
    {
      Serial1.println("  hindernis - true - rotiere links- ");
      Serial.println("  hindernis - true - rotiere links- ");
      rotate_left_deg();

      Serial1.println("  fahrt ausführen ");
      Serial.println("  fahrt ausführen  ");
      fahrt_ausfuehren();

---

Wenn das auch dauern kommt, liegt das Problem doch in hindernis_vorh.

ich habe den code ein wenig entrümpelt, nochmals alles überprüft und die hindernis variablenwerte angepasst, es funktioniert jetzt auch mit der funktion "hindernis_vorh()"...

video

- perfekt ist es noch nicht, aber prinzipiell ok. Eine drehung nach rechts funktioniert wegen der lage des US-sensors nicht so gut, muss noch angepasst werden...
- da wo der einer robby am hindernis hängen bleibt, da sind die radwellen ein paar milimeter zu lang :-)
- auch habe ich überlegt ob es möglich wäre beim schräg angefahrenem hindernis entsprechend "drehe links" beim hindernis auf der rechten seite und "drehe rechts" beim hindernis links zu reagieren. Der entsprechende lösungsansatz fehlt mir aber noch. Hat jemand eine idee?

Code:

---

#include <CustomStepper.h>
#include <NewPing.h>
#include <Wire.h>

#define TRIGGER_PIN  6
#define ECHO_PIN    7
#define MAX_DISTANCE 400

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

uint8_t idx;
uint16_t distanz;
uint16_t uS;
uint8_t zitter;

enum stepper_e
{ stepper_HM, stepper_VR, stepper_VL, stepper_MAX };


CustomStepper stepper[stepper_MAX]
{
  CustomStepper(35, 37, 39, 41),//stepper_HM-I
  CustomStepper(23, 25, 27, 29),//stepper_VR-II
  CustomStepper(47, 49, 51, 53) //stepper_VL-III
};

boolean start_ping;
boolean hindernis;



void setup()
{

  start_ping = false;
  hindernis = false;
  zitter = 1;

  Serial.begin(115200);
  Serial1.begin(115200);

}

void loop()
{

  hindernis_vorh();

  {
    vorwaerts();
    fahrt_ausfuehren();
  }

}
/***********************************************************/

void hindernis_vorh(void)
{
  if (start_ping == false) ping_distanz();
  if (uS != NO_ECHO)

  {
    if (((uS / US_ROUNDTRIP_CM) <= 20))
      hindernis = true;
  }
  else
  {
    hindernis = false;
  }
}
/***********************************************************/

void ping_distanz(void)
{
  uS = sonar.ping();
  Serial1.print("Ping: ");
  Serial1.print(uS / US_ROUNDTRIP_CM);
  Serial1.println(" cm");
  Serial.print("Ping: ");
  Serial.print(uS / US_ROUNDTRIP_CM);
  Serial.println(" cm");
  start_ping = true;
}
/***********************************************************/

void vorwaerts()
{
  if (start_ping == true)  ping_distanz();

  if (hindernis == true)
  {

    hindernis = false;

    //richtung_HM();
    zitter_li_re();
    fahrt_ausfuehren();

 
    //entspricht rotate_left_deg
    for (idx = stepper_HM; idx < stepper_MAX; idx++)
    {

      stepper[idx].setRPM(12);
      stepper[idx].setSPR(4075.7728395);
      stepper[idx].setDirection(CW);
      stepper[idx].rotateDegrees(120);
    }
    fahrt_ausfuehren();

    hindernis = false;
  }
  else

    hindernis = false;
  {

    zitter_li_re();
    fahrt_ausfuehren();

  }

  fahrt_ausfuehren();

}
/***********************************************************/

void zitter_li_re()
{
  stepper[stepper_HM].setRPM(12);
  stepper[stepper_VR].setRPM(12);
  stepper[stepper_VL].setRPM(12);

  stepper[stepper_HM].setSPR(4075.7728395);
  stepper[stepper_VR].setSPR(4075.7728395);
  stepper[stepper_VL].setSPR(4075.7728395);

  stepper[stepper_VL].setDirection(CCW);
  stepper[stepper_VL].rotateDegrees(30);

  stepper[stepper_VR].setDirection(CW);
  stepper[stepper_VR].rotateDegrees(30);https://youtu.be/JWn1sAYkfhc

  if (zitter == 1)
  {
    stepper[stepper_HM].setDirection(CCW);
    stepper[stepper_HM].rotateDegrees(25);//individuell anpassen
    zitter = 0;
  }
  else
  {
    stepper[stepper_HM].setDirection(CW);
    stepper[stepper_HM].rotateDegrees(25);//individuell anpassen
    zitter = 1;
  }
}
/***********************************************************/

void richtung_HM()
{
  stepper[stepper_HM].setRPM(12);
  stepper[stepper_VR].setRPM(12);
  stepper[stepper_VL].setRPM(12);

  stepper[stepper_HM].setSPR(4075.7728395);
  stepper[stepper_VR].setSPR(4075.7728395);
  stepper[stepper_VL].setSPR(4075.7728395);

  stepper[stepper_VL].setDirection(CW);
  stepper[stepper_VL].rotateDegrees(60);

  stepper[stepper_VR].setDirection(CCW);
  stepper[stepper_VR].rotateDegrees(60);
}

/***********************************************************/

boolean fahrt_fertig()
{
  return        stepper[stepper_HM].isDone() && stepper[stepper_VR].isDone()
                && stepper[stepper_VL].isDone();
}
/************************************************************/
void fahrt_ausfuehren()
{
  while ( ! fahrt_fertig() )
  {
    for (idx = stepper_HM; idx < stepper_MAX; idx++)
    {
      stepper[idx].run();
      // delay(1);
    }
  }
}

---