From 9b86a6dc3ddd0bbf0d356dc28521693c5e5bda26 Mon Sep 17 00:00:00 2001
From: "david.ihlenfeld" <david.ihlenfeld@smail.th-koeln.de>
Date: Tue, 14 May 2024 22:27:23 +0200
Subject: [PATCH] Fix Fehler Haltepunkt und Anpassung 'velocity'
---
Fahrzeug/bits3-bare-minimum/basic_functions.h | 106 ----------------
.../bits3-bare-minimum/bits3-bare-minimum.ino | 3 +-
Fahrzeug/bits3-bare-minimum/config.h | 4 +-
.../bits3-bare-minimum/custom_functions.h | 115 +++++++++++++++++-
4 files changed, 118 insertions(+), 110 deletions(-)
diff --git a/Fahrzeug/bits3-bare-minimum/basic_functions.h b/Fahrzeug/bits3-bare-minimum/basic_functions.h
index 19790ab..827ae1b 100644
--- a/Fahrzeug/bits3-bare-minimum/basic_functions.h
+++ b/Fahrzeug/bits3-bare-minimum/basic_functions.h
@@ -397,110 +397,4 @@ void drive_curve(float curve_angle, int dir) {
Motor.stop();
}
-/* ************* *
- Main-Funktion
- * ************* */
-
-void drive_course() { //Main-Funktion zum Abfahren des Kurses und anschliessendem Einparken (anhand der State-table)
-
- //Start Abfahren des Fahrkurses
- for (i = 0; i < ABSCHNITTE; i++) {
-
- //Abschnitts-Ausgabe
- //Serial.print("Aktueller Abschnitt: ");
- //Serial.println(state_table[i].part);
-
- float x_diff = state_table[i].x_kor - state_table[i - 1].x_kor; //Differenz aktuelle und vorherige x Koordinate
- float y_diff = state_table[i].y_kor - state_table[i - 1].y_kor; //Differenz aktuelle und vorherige y Koordinate
- int curve_dir; //Kurvenrichtung: ergibt sich durch Vergleich der Koordinaten-Differenz
- float curve_angle; //Kurvenwinkel: ergibt sich aus Berechung mit Koordinaten-Differenz
- float drive_dist; //zu fahrende gerade Strecke aus Differnez der Koordinaten (Betrag)
-
- //Berechnung curve_angle
- if (abs(x_diff) > 0.00) {
- curve_angle = 180 / PI * atan(x_diff / y_diff);
- }
- else if (abs(y_diff) > 0.00) {
- curve_angle = 180 / PI * atan(y_diff / x_diff);
- }
- else {
- curve_angle = 0.00;
- }
-
- //Bestimmung Kurvenrichtung: Vergleich der letzten beiden / letzten drei Koordinatenpunkte zur Bestimmung der Fzg.Ausrichtung
- if (state_table[i].x_kor > state_table[i - 1].x_kor) {
- if (state_table[i].y_kor > state_table[i - 2].y_kor) {
- curve_dir = 1;
- }
- else if (state_table[i].y_kor < state_table[i - 2].y_kor) {
- curve_dir = 2;
- }
- else {
- curve_dir = 0;
- }
- }
- else if (state_table[i].x_kor < state_table[i - 1].x_kor) {
- if (state_table[i].y_kor > state_table[i - 2].y_kor) {
- curve_dir = 2;
- }
- else if (state_table[i].y_kor < state_table[i - 2].y_kor) {
- curve_dir = 1;
- }
- else {
- curve_dir = 0;
- }
- }
- else if (state_table[i].y_kor > state_table[i - 1].y_kor) {
- if (state_table[i].x_kor > state_table[i - 2].x_kor) {
- curve_dir = 2;
- }
- else if (state_table[i].x_kor < state_table[i - 2].x_kor) {
- curve_dir = 1;
- }
- else {
- curve_dir = 0;
- }
- }
- else if (state_table[i].y_kor < state_table[i - 1].y_kor) {
- if (state_table[i].x_kor > state_table[i - 2].x_kor) {
- curve_dir = 1;
- }
- else if (state_table[i].x_kor < state_table[i - 2].x_kor) {
- curve_dir = 2;
- }
- else {
- curve_dir = 0;
- }
- }
-
- //Berechung Strecke Gerade
- if (abs(curve_angle) == 0.0) {
- drive_dist = abs(x_diff) + abs(y_diff); //Berechung des Betrags der Koordinatendifferenz (eine immer 0)
- }
- else {
- drive_dist = sqrt(sq(abs(x_diff)) + sq(abs(y_diff))); //Berechung der Hypotenuse eines Dreiecks (x- und y-Anteil der Koordinaten)
- }
-
- //Fahre Kurve
- drive_curve(abs(curve_angle), curve_dir);
-
- //Fahre Gerade: Uebergabe der berechneten Strecke mit Abzug eines Korrekturwertes (aufgrund des Kurvenradius vor und nach einer Gerade)
- if (i == 0) {
- drive_straight(drive_dist - (dist_korr)); //Im ersten Abschnitt nur einfache Korrektur (da zu Beginn keine Kurve)
- }
- else {
- drive_straight(drive_dist - (dist_korr * 2)); //In allen weiteren Abschnitten eine zweifache Korrektur
- }
-
-
- //Prüfe ob Einparken laut Zustandsautomat:
-
- //Parkplatzerkennung anhand der gegebenen Parkplatzgeometrie
- //automatisches Einparken mit Parkplatz-Seite und -Typ
-
-
- }
- end_of_programm(); //Programmende sobald alle Abschnitte durchfahren sind
-}
-
#endif
diff --git a/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino b/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
index 2d427de..c24fd31 100644
--- a/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
+++ b/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
@@ -119,5 +119,6 @@ void setup() {
* ************* */
void loop() {
- drive_course(); //Hier wird nur diese Funktion ausgeführt werden --> Erweitern Sie diese um ihren Funktionen!
+ drive_course(); //Hier wird nur diese Funktion ausgeführt werden --> Erweitern Sie diese um ihren Funktionen!
+ //Bearbeiten Sie die drive_course Funktion unter custom_functions und erweitern Sie diese an der gekennzeichneten Stelle um die Parkfunktionen
}
diff --git a/Fahrzeug/bits3-bare-minimum/config.h b/Fahrzeug/bits3-bare-minimum/config.h
index 6b53ab0..a197edd 100644
--- a/Fahrzeug/bits3-bare-minimum/config.h
+++ b/Fahrzeug/bits3-bare-minimum/config.h
@@ -74,8 +74,8 @@ uint8_t red, green, blue;
* Antriebsmotor *
* ************* */
bits_MotorDriverTB6612FNG Motor;
-uint8_t velocity = 100;
-uint8_t velocity_backward = 80;
+uint8_t velocity = 75;
+uint8_t velocity_backward = 60;
Encoder encoder(18, 19); //A,B: Pins, an die der Encoder angeschlossen ist
const float wheelDiameter = 0.0255; //empirisch ermittelt damit passend für Übersetzung (entspricht nicht Durchmesser des Rades)
diff --git a/Fahrzeug/bits3-bare-minimum/custom_functions.h b/Fahrzeug/bits3-bare-minimum/custom_functions.h
index 3e121de..1897c63 100644
--- a/Fahrzeug/bits3-bare-minimum/custom_functions.h
+++ b/Fahrzeug/bits3-bare-minimum/custom_functions.h
@@ -6,5 +6,118 @@ void platzhalter(){
// Dies ist nur ein Platzhalter.
}
+/* ************* *
+ Main-Funktion
+ * ************* */
-#endif
\ No newline at end of file
+void drive_course() { //Main-Funktion zum Abfahren des Kurses (anhand der State-table)
+
+ //Start Abfahren des Fahrkurses
+ for (i = 0; i < ABSCHNITTE; i++) {
+
+ //Abschnitts-Ausgabe
+ //Serial.print("Aktueller Abschnitt: ");
+ //Serial.println(state_table[i].part);
+
+ float x_diff = state_table[i].x_kor - state_table[i - 1].x_kor; //Differenz aktuelle und vorherige x Koordinate
+ float y_diff = state_table[i].y_kor - state_table[i - 1].y_kor; //Differenz aktuelle und vorherige y Koordinate
+ int curve_dir; //Kurvenrichtung: ergibt sich durch Vergleich der Koordinaten-Differenz
+ float curve_angle; //Kurvenwinkel: ergibt sich aus Berechung mit Koordinaten-Differenz
+ float drive_dist; //zu fahrende gerade Strecke aus Differnez der Koordinaten (Betrag)
+
+ //Berechnung curve_angle
+ if (abs(x_diff) > 0.00) {
+ curve_angle = 180 / PI * atan(x_diff / y_diff);
+ }
+ else if (abs(y_diff) > 0.00) {
+ curve_angle = 180 / PI * atan(y_diff / x_diff);
+ }
+ else {
+ curve_angle = 0.00;
+ }
+
+ //Bestimmung Kurvenrichtung: Vergleich der letzten beiden / letzten drei Koordinatenpunkte zur Bestimmung der Fzg.Ausrichtung
+ if (state_table[i].x_kor > state_table[i - 1].x_kor) {
+ if (state_table[i].y_kor > state_table[i - 2].y_kor) {
+ curve_dir = 1;
+ }
+ else if (state_table[i].y_kor < state_table[i - 2].y_kor) {
+ curve_dir = 2;
+ }
+ else {
+ curve_dir = 0;
+ }
+ }
+ else if (state_table[i].x_kor < state_table[i - 1].x_kor) {
+ if (state_table[i].y_kor > state_table[i - 2].y_kor) {
+ curve_dir = 2;
+ }
+ else if (state_table[i].y_kor < state_table[i - 2].y_kor) {
+ curve_dir = 1;
+ }
+ else {
+ curve_dir = 0;
+ }
+ }
+ else if (state_table[i].y_kor > state_table[i - 1].y_kor) {
+ if (state_table[i].x_kor > state_table[i - 2].x_kor) {
+ curve_dir = 2;
+ }
+ else if (state_table[i].x_kor < state_table[i - 2].x_kor) {
+ curve_dir = 1;
+ }
+ else {
+ curve_dir = 0;
+ }
+ }
+ else if (state_table[i].y_kor < state_table[i - 1].y_kor) {
+ if (state_table[i].x_kor > state_table[i - 2].x_kor) {
+ curve_dir = 1;
+ }
+ else if (state_table[i].x_kor < state_table[i - 2].x_kor) {
+ curve_dir = 2;
+ }
+ else {
+ curve_dir = 0;
+ }
+ }
+
+ //Berechung Strecke Gerade
+ if (abs(curve_angle) == 0.0) {
+ drive_dist = abs(x_diff) + abs(y_diff); //Berechung des Betrags der Koordinatendifferenz (eine immer 0)
+ }
+ else {
+ drive_dist = sqrt(sq(abs(x_diff)) + sq(abs(y_diff))); //Berechung der Hypotenuse eines Dreiecks (x- und y-Anteil der Koordinaten)
+ }
+
+ //Fahre Kurve
+ drive_curve(abs(curve_angle), curve_dir);
+
+ //Fahre Gerade: Uebergabe der berechneten Strecke mit Abzug eines Korrekturwertes (aufgrund des Kurvenradius vor und nach einer Gerade)
+ if (i == 0) {
+ drive_straight(drive_dist - (dist_korr)); //Im ersten Abschnitt nur einfache Korrektur (da zu Beginn keine Kurve)
+ }
+ else {
+ drive_straight(drive_dist - (dist_korr * 2)); //In allen weiteren Abschnitten eine zweifache Korrektur
+ }
+
+
+ //Prüfe ob Einparken laut Zustandsautomat:
+ if (state_table[i].parking_side != 0 && state_table[i].parking_type != 0) {
+
+ //Hier Parkfunktionen entwickeln
+ //Parkplatzerkennung anhand der gegebenen Parkplatzgeometrie
+ //automatisches Einparken mit Parkplatz-Seite und -Typ
+
+ end_of_programm();
+
+ }
+
+ }
+
+ end_of_programm();
+
+}
+
+
+#endif
--
GitLab