Hausaufgaben7 Tipp 1

Zuerst wird eine Funktion sucheZiel() programmiert, die den Arbeitsbereich 0°..180° absucht und das räumlich nächste Ziel zurück gibt.

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



void setup()                      // Einmalige Systeminitialisierung
{
  int PotiWinkel_s16 = 0;         // Winkel des Potis
  int DrehWinkel_s16 = 0;         // Drehwinkel des Roboters
  float USAbstand_f32 = NaN;

  Serial.begin(COM_GESCHW_s16);   // Serieller Monitor mit 9600 Baud

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

  PotiWinkel_s16 = sucheZiel(&USAbstand_f32);   // Nächstes Ziel wird gesucht

  Serial.print("Nächste Ziel: ");
  Serial.print(USAbstand_f32);
  Serial.print(" m");

} // END setup()


void loop() // Main Schleife
{           // Hier passiert nichts.
}

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);
    *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()

---

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