diff --git a/Fahrzeug/README.md b/Fahrzeug/README.md
index eeee850709c3c0f30401fb0c6da8fb6c0eb12d71..93b34fdf5d00a2bb1f6e93b0c066e70a6808ac79 100644
--- a/Fahrzeug/README.md
+++ b/Fahrzeug/README.md
@@ -44,6 +44,15 @@ course_state_t state_table[] = {
{4, 0.00, 0.00, 1, 0, 0}, // 2 = linke Seite
{5, 0.00, 0.00, 1, 0, 0} //Parken 1: vorwaerts , 2: rueckwaerts , 3: seitlich
}
+
+//Die Zustandstabelle des vollstaendigen BITS3 Parkhaus sieht wie folgt aus:
+course_state_t state_table[] = {
+ {1, 0.00, 0.70, 0, 0, 0},
+ {2, 2.00, 0.70, 2, 0, 0},
+ {3, 2.00, 2.40, 1, 0, 0},
+ {4, 0.00, 2.40, 1, 0, 0},
+ {5, 0.00, 0.70, 1, 0, 0}
+};
```
### Fahrzeug Parameter
diff --git a/Fahrzeug/bits3-bare-minimum/basic_functions.h b/Fahrzeug/bits3-bare-minimum/basic_functions.h
index 827ae1b30bbf7c002ef2275d18959691dabc06a2..5a161ac14abcb243ad95557ee4df3ed5260d1f80 100644
--- a/Fahrzeug/bits3-bare-minimum/basic_functions.h
+++ b/Fahrzeug/bits3-bare-minimum/basic_functions.h
@@ -338,21 +338,26 @@ void proof_drive() {
Motor.stop(); //falls ja, ausschalten des Motor und aktivieren der Warnblinke (5 sek)
warn_signal();
}
- v = v_start;; //Setzen einer Startgeschwindigkeit, damit kein Feststecken in Logikschleife
+ v = v_start; //Setzen einer Startgeschwindigkeit, damit kein Feststecken in Logikschleife
}
//Regelung für die Fahrt parallel zu einer seitlichen Wand (abh. von Fahrbahnbreite)
void course_correction_side(int side) {
+ //Zeitintervall seit letzter Regelung bestimmen
+ now = millis();
+ dt = (now - last_time) / 1000;
+ last_time = now;
+
if (side == 1) { //Korrektur auf rechter Seite
tof_r_1 = tof_controller.get_distance_mm(2); //Nutzung des vorderen rechten TOF Sensors
wall_difference = tof_r_1 - wall_distance; //Regelung erfolgt anhand eines Soll-Abstandes
- output_value = pid.compute(wall_difference);
+ output_value = pid.compute(wall_difference, dt);
servo_steering_angle = STEER_START_POS - output_value;
}
else if (side == 2) { //Korrektur auf linker Seite
tof_l_1 = tof_controller.get_distance_mm(1); //Nutzung des vorderen linken TOF Sensors
wall_difference = tof_l_1 - wall_distance;
- output_value = pid.compute(wall_difference);
+ output_value = pid.compute(wall_difference, dt);
servo_steering_angle = STEER_START_POS + output_value;
}
else { //wird 0 anstatt 1 oder 2 übergeben erfolgt keine Regelung
diff --git a/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino b/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
index c24fd31f74b8f80c5ba56d7d58f85af187867910..7f3770a180e933aaf0e83662487ba808606c7b4c 100644
--- a/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
+++ b/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
@@ -12,13 +12,16 @@
- Name des Skripts
Description:
- - Beschrebiung was dieses Skript macht
+ - Beschreibung was dieses Skript macht
Author:
- Namen der Autoren
**************************************************/
+// Das folgende define Statement auskommentieren, um die Funktion des Fahrzeugs ohne Schranke zu testen
+#define SCHRANKE
+
/* ******************************* *
Kommunikation des Arduino Megas
* ******************************* */
@@ -94,8 +97,8 @@ void setup() {
// Initialisierung abgeschlossen
_println_("Ready!");
- // buzzer_signal(350, 2, 100); //tone() und IR-Empfangen läuft nicht gleichzeitig!!!!
+#ifdef SCHRANKE
// Heranfahren an Schranke
drive_light_on();
ServoSteer.turn(STEER_START_POS);
@@ -111,8 +114,11 @@ void setup() {
set_led_color(0, 100, 0);
all_led_blink(2, 200);
delay(3000); // Warten bis Schranke offen ist
-}
+#endif
+
+ pid_start = millis();
+}
/* ************* *
Hauptprogramm
diff --git a/Fahrzeug/bits3-bare-minimum/config.h b/Fahrzeug/bits3-bare-minimum/config.h
index a197edddaeed27d3f95907c4a92a23e051e10f9e..3195e0ae1593f563deab35e8caae08b3d0ed5ab2 100644
--- a/Fahrzeug/bits3-bare-minimum/config.h
+++ b/Fahrzeug/bits3-bare-minimum/config.h
@@ -85,9 +85,9 @@ float distance = 0.00; //Distanz [m] die das Fahrzeug zurück gelegt
* Servomotor Lenkung *
* ****************** */
const int STEER_SERVO_PIN = 11;
-const int STEER_ANGLE_MAX = 138; // Muss eventuell angepasst werden!
-const int STEER_ANGLE_MIN = 64; // Muss eventuell angepasst werden!
-const int STEER_START_POS = 93; // Muss eventuell angepasst werden!
+const int STEER_ANGLE_MAX = 110; // Muss angepasst werden!!!
+const int STEER_ANGLE_MIN = 70; // Muss angepasst werden!!!
+const int STEER_START_POS = 90; // Muss angepasst werden!!!
thk_ServoController ServoSteer(STEER_SERVO_PIN, STEER_START_POS, STEER_ANGLE_MAX, STEER_ANGLE_MIN);
int STEER_HALF_RIGHT = STEER_START_POS + (STEER_ANGLE_MAX-STEER_START_POS)/2;
@@ -199,6 +199,10 @@ const double diff_gain = 0.0; //D-Glied
uint8_t setpoint = 0; //Zielwert Regeldifferenz
double wall_difference; //Regelgroeße: Differenz zwischen TOF-Wert und Soll Wert
double output_value; //korrigierter Lenkwinkel durch PID
+double last_time = 0.0; //Zeitvariable zum berechnen von dt
+double now = 0.0; //Zeitvariable zum berechnen von dt
+double dt = 0.0; //Zeit der Regelstrecke
+double pid_start = 0.0; //Startzeit zur pid MESSUNG
thk_PIDController pid(prop_gain, int_gain, diff_gain, 0);
double left_right_difference; //Differenz TOF vorne
@@ -223,11 +227,11 @@ typedef struct course_state_t
// Hier wird der Fahrkurs in einer Tabelle für das Fahrzeug definiert:
// Leere Zustandstabelle, Daten kommen mit Hilfe von Funk/IR von Parkplatzinfrastruktur
course_state_t state_table[] = {
- {1, 0.00, 0.00, 0, 0, 0},
- {2, 0.00, 0.00, 2, 0, 0},
- {3, 0.00, 0.00, 1, 0, 0},
- {4, 0.00, 0.00, 1, 0, 0},
- {5, 0.00, 0.00, 1, 0, 0}
+ {1, 0.00, 0.00, 0, 0, 0},
+ {2, 0.00, 0.00, 2, 0, 0},
+ {3, 0.00, 0.00, 1, 0, 0},
+ {4, 0.00, 0.00, 1, 0, 0},
+ {5, 0.00, 0.00, 1, 0, 0}
};
const int ABSCHNITTE = sizeof(state_table) / sizeof(course_state_t); //Größe der Tabelle (Anzahl der Abschnitte siehe Parklayout)
diff --git a/Fahrzeug/bits3-bare-minimum/custom_config.h b/Fahrzeug/bits3-bare-minimum/custom_config.h
index 8ed825e906e5bb5ec07567408ce63139e8e3ea41..be9611b1de61e75e21bdb901e0ca6a87e769ca8a 100644
--- a/Fahrzeug/bits3-bare-minimum/custom_config.h
+++ b/Fahrzeug/bits3-bare-minimum/custom_config.h
@@ -1,6 +1,20 @@
#ifndef CUSTOMCONFIG_H
#define CUSTOMCONFIG_H
-int pl_halter = 0;
+#ifndef SCHRANKE
+//Diese Statetable kann zum testen der Reglerfunktion an einer beliebigen Strecke verwendet werden.
+//Die verwendete Strecke muss dazu vermessen werden und die Abschnitte müssen sorgtfältig in die Statetable eingetragen werden.
+//Lesen Sie die Readme für mehr Informationen zur Statetable.
+ course_state_t state_table[] = {
+ {1, 0.00, 0.00, 0, 0, 0},
+ {2, 0.00, 0.00, 2, 0, 0}, //1 = rechts
+ {3, 0.00, 0.00, 1, 0, 0}, //2 = links
+ {4, 0.00, 0.00, 1, 0, 0},
+ {5, 0.00, 0.00, 1, 0, 0}
+ };
+#endif
+
+int pl_halter = 0
+
#endif
\ No newline at end of file