Hallo zusammen,
ich habe ein ferngesteuertes Auto mit einem Arduino uno gebaut und habe Probleme bei einer Notbremsung, die durch einen Ultraschallsensor ausgelöst wird.
Die Steuerung funktioniert über eine Fernbedieung, die ebenfalls über einen Arduino uno läuft. Ich habe zwei Joytsicks für den Motor und die Lenkung angeschlossen und übertrage die Daten über ein Funkmodul. Die Steuerung des Autos funktioniert einwandfrei, ich habe nur Probleme bei der Notbremsung.
Dort funktioniert die Logik folgendermaßen:
Über den Ultraschallsensor messe ich den Abstand zu einem Objekt, welches sich in Fahrtrichtung befindert. Über die Änderung des Abstands kann ich auch die aktuelle Geschwindigkeit berechnen. Ich möchte, dass das Auto automatisch eine Notbremsung auslöst, wenn die Geschwindigkeit zu hoch ist. Das mache ich, indem ich das Verhältnis aus Abstand und Geschwindigkeit berechne. Wenn dieses zu Gering ist, wird die Bremsung eingeleitet. Dass der Bremsweg exponentiell mit der Geschwindigkeit steigt ignoriere ich vorerst. Ob eine Bremsung eingeleitet wird, überprüfe ich nur dann, wenn ein Obejekt weniger als einen Meter entfernt ist und das Auto aktuell vorwärts fährt.
Soweit zur Theorie. Leider funktioniert meine If-Verzweigung nicht richtig. Das Programm führt eine Notbremsung aus, sobald der Abstand zu einem Objekt kleiner als ein Meter ist. Auch dann, wenn sich das Auto nicht bewegt. Ich lasse mir die Zwischenwerte ausgeben und die Werte für Abstand und Geschwindigkeit stimmen. Auch der Faktor ist richtig (also deutlich über dem "kritischen" Wert), trotzdem wird die Schleife für die Notbremsung durchlaufen, obwohl die Bedingung nicht erfüllt ist.
Das Problem ist also zusammengefasst, dass permanent eine Notbremsung ausgeführt wird, sobald der Abstand eines Objekts kleiner als 1 Meter ist.
Ich weiß, es wird immer nur ausgeführt, was ich Programmiere, aber ich sehe bei den Werten, die ausgegeben werden, eigentlich keinen Fehler.
Ich befürchte einen Fehler bei den Datentypen, aber weiß einfach nicht mehr weiter.
Ich hoffe jemand hat die Zeit und kann mir weiterhelfen, dafür wäre ich sehr dankbar. Da ich eher noch Anfänger bin, habe ich wahrscheinlich irgendwas offensichtliches Übersehen.
//Bibliotheken
#include <PWMServo.h> //Motor Shield
#include "SR04.h" //Ultraschall
#include <SPI.h> //SPI-Schnittstelle für Funkmodul
#include <nRF24L01.h> //Funkmoduk
#include <RF24.h> //Funkmodul
//Zeiten für Delays
unsigned long previousMillis = 0;
const long interval = 150; // Länge eines Intervalls
//Abstand, Geschwindigkeit, Bremsen
long previousAbstand = 0;
long currentspeed = 0;
long breakpower = 0;
long factor = 100;
//Motor
const int Motor1_In1 = 7;
const int Motor1_In2 = 8;
const int Motor1_Enable = 5;
//Ultraschallsensor
const int TRIG_PIN = 3;
const int ECHO_PIN = 2;
SR04 Ultraschall = SR04(ECHO_PIN, TRIG_PIN);
int Abstand;
//Servo
PWMServo Servo_Steering; //Servo-Objekt erstellen
const int Servo_PIN_Steering = 9;
//Empfänger für die Signale der Fernbedienung
RF24 radio(6, 4); // CE, CSN
const byte addresses[][6] = {"00001", "00002"};
//Klasse für Empfänger
struct Packet
{
int x;
int y;
} pack;
void setup() {
Serial.begin(9600);
//Empfänger
radio.begin();
radio.openWritingPipe(addresses[0]); // 00001
radio.openReadingPipe(1, addresses[1]); // 00002
radio.setPALevel(RF24_PA_MIN);
//radio.startListening();
//Motor
pinMode(Motor1_Enable, OUTPUT);
pinMode(Motor1_In1, OUTPUT);
pinMode(Motor1_In2, OUTPUT);
//Servo
Servo_Steering.attach(Servo_PIN_Steering);
Servo_Ultraschall.attach(Servo_PIN_Ultraschall);
Serial.begin(9600);
}
void loop() {
//Um fehlerhafte Werte auszumustern
int Abstand_gemessen = Ultraschall.Distance();
if (Abstand_gemessen < 1000) {
Abstand = Abstand_gemessen;
}
Serial.print("Abstand : ");
Serial.println(Abstand);
//Überprüfen, ob Intervall abgelaufen ist
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
//Geschwindigkeit berechnen
currentspeed = 1000 * (previousAbstand - Abstand) / interval;
//Einheit [cm/s], 1000 für Umrechnung Millisekunden -> Sekunden
previousAbstand = Abstand;
}
Serial.print("currentspeed : ");
Serial.println(currentspeed);
//Daten empfangen
radio.startListening(); //Empfänger und Sender
if (radio.available()) {
while (radio.available()) {
Packet packet;
radio.read(&packet, sizeof(packet));
int power_out = packet.y;
int steering_out = packet.x;
//Lenkung
turn(steering_out);
//Notbremse
factor = 100 * Abstand / (abs(currentspeed) + 0.11);
//Über den Faktor wird berechnet, ob der Bremsweg bei der aktuellen Geschwindigkeit größer ist, als der aktuelle Abstand zu einem Objekt
Serial.print("factor : ");
Serial.println(factor);
//In der folgenden Schleife hängt sich das Programm auf
//Wenn man vorwärts fährt und ein Objekt in der Nähe ist
if (Abstand < 100 && currentspeed > 0) {
//Vollbremsung
if (factor < 25) {
power_out = -180;
}
//Langsame Bremsung; Werte liegen zwischen 0 für Leerlauf und -180 für Vollbremsung. Durch "map" wird die Bremspower an den factor angepasst
else if (25 <= factor <= 50) {
breakpower = map(factor, 50, 25, 0, -180)
power_out = breakpower;
Serial.print("breakpower : ");
Serial.println(breakpower);
}
}
Serial.print("power_out, ENDE : ");
Serial.println(power_out);
go(power_out);
}
radio.stopListening(); //Empfänger und Sender
//radio.write(¤tspeed, sizeof(currentspeed)); //Empfänger und Sender
//radio.write(&power_out, sizeof(power_out)); //Empfänger und Sender
}
}
//________________________________________________________________________
//Funktionen
void go(int power_out)
{
if (power_out >= 0) {
digitalWrite(Motor1_In1, LOW);
digitalWrite(Motor1_In2, HIGH);
analogWrite(Motor1_Enable, power_out);
}
else if (power_out < 0) {
digitalWrite(Motor1_In1, HIGH);
digitalWrite(Motor1_In2, LOW);
analogWrite(Motor1_Enable, abs(power_out));
}
//Serial.println(power_out);
}
//Steering
void turn(int steering_out)
{
Servo_Steering.write(steering_out);
int steering_out_Ultraschall = map(steering_out, 123, 56, 56, 123);
Servo_Ultraschall.write(steering_out_Ultraschall);
}
Danke und Grüße
Sebi
