PID Anpassung, Einfache Test ohne Schranke möglich

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

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

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

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

Fahrzeug/bits3-bare-minimum/custom_config.h

 #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