- Labornetzteil AliExpress         
Ergebnis 1 bis 7 von 7

Thema: hindernis erkennung omni-move

  1. 07.10.2016, 13:46 #1
    inka
    inka ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    77
    Beiträge
    2.180

    hindernis erkennung omni-move

    Anzeige

     E-Bike
    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...
    gruß inka
    ZitierenZitieren
  2. 07.10.2016, 14:34 #2
    Mxt
    Mxt ist offline
    Erfahrener Benutzer Roboter Experte
    Registriert seit
    04.09.2011
    Ort
    Hessen
    Beiträge
    707
    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 ?
    ZitierenZitieren
  3. 07.10.2016, 14:55 #3
    inka
    inka ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    77
    Beiträge
    2.180
    diese funktion habe ich bereits in diesem thread zum 4-rad robby verwendet, dort funktioniert sie einwandfrei...
    gruß inka
    ZitierenZitieren
  4. 07.10.2016, 15:29 #4
    Mxt
    Mxt ist offline
    Erfahrener Benutzer Roboter Experte
    Registriert seit
    04.09.2011
    Ort
    Hessen
    Beiträge
    707
    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.
    ZitierenZitieren
  5. 07.10.2016, 18:26 #5
    inka
    inka ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    77
    Beiträge
    2.180
    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...
    gruß inka
    ZitierenZitieren
  6. 08.10.2016, 08:26 #6
    Mxt
    Mxt ist offline
    Erfahrener Benutzer Roboter Experte
    Registriert seit
    04.09.2011
    Ort
    Hessen
    Beiträge
    707
    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.
    Geändert von Mxt (08.10.2016 um 08:47 Uhr)
    ZitierenZitieren
  7. 12.10.2016, 11:14 #7
    inka
    inka ist offline
    Erfahrener Benutzer Robotik Einstein Avatar von inka
    Registriert seit
    29.10.2006
    Ort
    nahe Dresden
    Alter
    77
    Beiträge
    2.180
    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);
    }
  }
}
    gruß inka
    ZitierenZitieren
« Vorheriges Thema | Nächstes Thema »

Ähnliche Themen

  1. omni move roboter
    Von inka im Forum Vorstellungen+Bilder von fertigen Projekten/Bots
    Antworten: 0
    Letzter Beitrag: 05.09.2016, 17:09
  2. RP6 Move
    Von Marian Otte im Forum Robby RP6
    Antworten: 7
    Letzter Beitrag: 05.02.2013, 15:07
  3. RP6: Probleme mit I2C und move
    Von -schumi- im Forum Robby RP6
    Antworten: 9
    Letzter Beitrag: 26.06.2012, 17:22
  4. Hindernis-Erkennung und Drehung des Roboters
    Von Death_N00b im Forum Sensoren / Sensorik
    Antworten: 12
    Letzter Beitrag: 01.05.2011, 14:43
  5. kombination von fahren(move) und servobew. (getStopwatch)
    Von inka im Forum Robby RP6
    Antworten: 4
    Letzter Beitrag: 01.09.2010, 13:34

Lesezeichen

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •  

Foren-Regeln


12V Akku bauen