Hausaufgaben7 Lösung

Letztendlich werden alle Teile zusammengesetzt:

• Suche nach dem Ziel.
• Rotation des Roboters in Richtung des Ziels.
• Stopp-Funktion.

Quelltext

/* Bibliotheken hinzufügen */
#include "AlphaBot.h"          // Header hinzufügen
#include <Servo.h>

/* Instanzen erzeugen */
AlphaBot Alf = AlphaBot();   // Instanz des AlphaBot mit dem Handle WallE erzeugen
Servo AlfServo;              // Servo Instanz

/* KONSTANTEN deklarieren */
const int TRIG = 11;                     // US Trigger an D11
const int ECHO = 12;                     // US Echo an D12
static const byte SERVO_PORT_u8 = 9;     // Port des Servo Motors
static const int  COM_GESCHW_s16 = 9600; // Baude
static const float NaN = 1029;           // NaN beim US, keine Objekt in Reichweite


// Globale Variablen
//boolean WinkelErreicht_b = false; // true, wenn 30 cm gefahren

void setup()                      // Einmalige Systeminitialisierung
{
  Serial.begin(COM_GESCHW_s16);   // Serieller Monitor mit 9600 Baud

  int PotiWinkel_s16 = 0;         // Winkel des Potis
  int DrehWinkel_s16 = 0;         // Drehwinkel des Roboters
  float USAbstand_f32 = NaN;

  initUltraschall();              // US initialisieren
  AlfServo.attach(SERVO_PORT_u8);    // Servo verbinden

  


  /* Suche Ziel */
  PotiWinkel_s16 = sucheZiel(&USAbstand_f32);   // Nächstes Ziel wird gesucht
  Serial.print("Nächste Ziel: ");
  Serial.print(USAbstand_f32);
  Serial.print(" m");
  do{
  /* Drehe Roboter */
  DrehWinkel_s16 = PotiWinkel_s16-90;          // Links: +
  dreheRobot(DrehWinkel_s16); // Links: +

  /* Suche Ziel */
  PotiWinkel_s16 = sucheZiel(&USAbstand_f32);   // Nächstes Ziel wird gesucht
  Serial.print("Nächste Ziel: ");
  Serial.print(USAbstand_f32);
  Serial.println(" m");
  DrehWinkel_s16 = PotiWinkel_s16-90;          // Links: +
  } while(abs(DrehWinkel_s16) > 5);            // Abbruch der Schleife
  Serial.print("* Drehwinkel: ");
  Serial.println(DrehWinkel_s16);
  AlfServo.write(PotiWinkel_s16);              // Finale Servoposition anfahren
  Alf.MotorRun(90,90);                         // Vorwärts auf das Ziel zufahren

}
void loop()                        // Main Schleife
{
  static const byte SCHWELLWERT_u8 = 10;   // Entfernungsschwellwert in cm
  float USAbstand_f32 = 0;
  USAbstand_f32 = messeUSAbstand();    // US Abstand messen 
  Serial.println(USAbstand_f32);
  if (USAbstand_f32 < float(SCHWELLWERT_u8)) // Schwellwert unterschritten?
  {
    Serial.println("Notbremsung");
    Alf.Brake();                       // Anhalten
  } 
}

/* ---------------------------------------------------------------*/
void initUltraschall()
/* ---------------------------------------------------------------*/
{
  pinMode(ECHO, INPUT);                   // Define the ultrasonic echo input pin
  pinMode(TRIG, OUTPUT);                  // Define the ultrasonic trigger input pin
  Serial.println("** initUltraschall");
}

/* ---------------------------------------------------------------*/
int sucheZiel(float *USAbstand_f32)
/* ---------------------------------------------------------------*/
{
  /* Lokale Variablen deklarieren */
  boolean ZielGefunden_bit = false;
  int ServoPosition_s16    = 0;
  int ServoInkrement_s16    = 5;
  float MinUSAbstand_f32 = NaN; // kleinster gemessener Abstand in cm, 1029 = NaN
  int MinServoPosition_s16 = NaN; // kleinster gemessener Abstand in cm, 1029 = NaN

  AlfServo.write(ServoPosition_s16); // Initiale Servoposition anfahren

  while (ZielGefunden_bit == false)
  {
    if (ServoPosition_s16 >= 0 && ServoPosition_s16 <= 180)
    {
      AlfServo.write(ServoPosition_s16);   // Der Servo soll sich an die Position drehen
    }
    delay(200);                            // Servo-Drehung abwarten
    *USAbstand_f32 = messeUSAbstand();     // US Abstand messen

    //delay(250);/* Wir warten 15ms, bis der Servo seine Position erreich hat */
    Serial.print("Winkel: ");
    Serial.print(ServoPosition_s16);
    Serial.print(" - ");
    Serial.print(*USAbstand_f32);
    if (*USAbstand_f32 > 2 && *USAbstand_f32 < 400) // nur valide Messungen betrachten
    {
      if (*USAbstand_f32 < MinUSAbstand_f32)
      {
        MinUSAbstand_f32 = *USAbstand_f32;
        MinServoPosition_s16 = ServoPosition_s16;
        Serial.print(" * ");                       // räuml. nächstes Ziel markieren
      }
    }
    Serial.println(" cm");
    ServoPosition_s16 += ServoInkrement_s16;
    if (ServoPosition_s16 < 0)
    {
      ServoPosition_s16 = 0;
      ServoInkrement_s16 *= (-1);                 // Am Ende des Sweeps in die andere Richtung drehen
      if (MinUSAbstand_f32 < NaN)                 // Wurde ein Ziel gefunden?
      {
        ZielGefunden_bit = true;
      }
      Serial.println(ServoInkrement_s16);
    }
    else if (ServoPosition_s16 > 180)
    {
      ServoPosition_s16 = 180;
      ServoInkrement_s16 *= (-1);                // Am Ende des Sweeps in die andere Richtung drehen
      if (MinUSAbstand_f32 < NaN)                // Wurde ein Ziel gefunden?
      {
        ZielGefunden_bit = true;
      }
      Serial.println(ServoInkrement_s16);
    }
  }
  /* Rückgabewerte */
  *USAbstand_f32 = MinUSAbstand_f32;             // Mindestanstand
  return MinServoPosition_s16;                   // ServoPosition
} // END sucheZiel

/* ---------------------------------------------------------------*/
float messeUSAbstand()                // Abstand in cm
/* ---------------------------------------------------------------*/
{
  float USAbstand = 0;                    // Rückgabewert initialisieren
  float fSignalLaufzeit = 0;              // Messwert der Signallaufzeit

  digitalWrite(TRIG, LOW);                // setze Trigger auf LOW für 2μs
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);               // setze Trigger auf HIGH für mind. 10 us
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);                // setze Trigger auf LOW
  fSignalLaufzeit = pulseIn(ECHO, HIGH);  // Messung der Singnallaufzeit des Impulses

  USAbstand = fSignalLaufzeit / 58;       // Umrechnung Laufzeit in s in  Abstand in cm
  //Y m =（X s * 344）/ 2;
  //X s =（ 2 * Y m）/ 344;
  //X s = 0.0058 * Y m;
  //cm = us /58
  return USAbstand;                       // Rückgabewert
} // END  messeUSAbstand()

/* ---------------------------------------------------------------*/
void dreheRobot(int DrehWinkel_s16){ // Eingang Winkel in deg, 
/* ---------------------------------------------------------------*/
  /* Winkelbereich -90°..90° */
  int nPower = 100;  // Default: 90
  int Delay_s16 = 0;
  if(DrehWinkel_s16<-90 || DrehWinkel_s16>90){
    Serial.println("ERROR: Winkel außerhalb des gültigen Bereichs.");
  }
  else{
                                         // Sollwinkel: 
  Delay_s16=abs(DrehWinkel_s16)*40/9;      // Platzdrehung abwarten Power: 90, 400ms -> 90°
  Serial.print("Delay: ");
  Serial.print(Delay_s16);
  if (DrehWinkel_s16<0){                     // 
     nPower = nPower*(-1);               // Winkel_s16<0: Rechtsdrehung
  }
  Alf.MotorRun(-nPower,+nPower);          // Alf Drehung
  delay(Delay_s16);                                                       
  Alf.Brake();
  Serial.print(" - Drehung: ");
  Serial.println(DrehWinkel_s16);
  }
}
  

---

→ zurück zum Hauptartikel: Termin 7: Ultraschall Notbremse