The derivative part increased for the last 2 parameter sets. Its nece…

…ssary for the turns. The gap detection reacts now instant on the position value. Otherwise the PID continued and the robot got a kick into a bad direction. Don't constrain for top speed, but for max. speed. This supports better behavior in turns.
BlueAndi · Nov 18, 2023 · 476b808 · 476b808
1 parent a70418e
commit 476b808
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 12 deletions.
diff --git a/lib/APPLineFollower/DrivingState.cpp b/lib/APPLineFollower/DrivingState.cpp
@@ -75,7 +75,6 @@ void DrivingState::entry()
     m_pidProcessTime.start(0); /* Immediate */
     m_lineStatus  = LINE_STATUS_FIND_START_LINE;
     m_trackStatus = TRACK_STATUS_ON_TRACK; /* Assume that the robot is placed on track. */
-    m_posMovAvg.clear();
 
     diffDrive.enable();
 
@@ -99,8 +98,6 @@ void DrivingState::process(StateMachine& sm)
     /* Get the position of the line. */
     position = lineSensors.readLine();
 
-    (void)m_posMovAvg.write(position);
-
     switch (m_trackStatus)
     {
     case TRACK_STATUS_ON_TRACK:
@@ -159,7 +156,7 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa
     }
 
     /* Track lost just in this moment? */
-    if (true == isTrackGapDetected(m_posMovAvg.getResult()))
+    if (true == isTrackGapDetected(position))
     {
         m_trackStatus = TRACK_STATUS_LOST;
 
@@ -332,6 +329,7 @@ void DrivingState::adaptDriving(int16_t position)
 {
     DifferentialDrive&  diffDrive       = DifferentialDrive::getInstance();
     const ILineSensors& lineSensors     = Board::getInstance().getLineSensors();
+    const int16_t       MAX_MOTOR_SPEED = diffDrive.getMaxMotorSpeed();
     int16_t             speedDifference = 0; /* [steps/s] */
     int16_t             leftSpeed       = 0; /* [steps/s] */
     int16_t             rightSpeed      = 0; /* [steps/s] */
@@ -357,8 +355,8 @@ void DrivingState::adaptDriving(int16_t position)
      * might want to allow the motor speed to go negative so that
      * it can spin in reverse.
      */
-    leftSpeed  = constrain(leftSpeed, 0, m_topSpeed);
-    rightSpeed = constrain(rightSpeed, 0, m_topSpeed);
+    leftSpeed  = constrain(leftSpeed, 0, MAX_MOTOR_SPEED);
+    rightSpeed = constrain(rightSpeed, 0, MAX_MOTOR_SPEED);
 
     diffDrive.setLinearSpeed(leftSpeed, rightSpeed);
 }

diff --git a/lib/APPLineFollower/DrivingState.h b/lib/APPLineFollower/DrivingState.h
@@ -47,7 +47,6 @@
 #include <IState.h>
 #include <SimpleTimer.h>
 #include <PIDController.h>
-#include <MovAvg.hpp>
 
 /******************************************************************************
  * Macros
@@ -131,7 +130,6 @@ class DrivingState : public IState
     LineStatus             m_lineStatus;  /**< Status of start-/end line detection */
     TrackStatus            m_trackStatus; /**< Status of track which means on track or track lost, etc. */
     uint8_t m_startEndLineDebounce;       /**< Counter used for easys debouncing of the start-/end line detection. */
-    MovAvg<int16_t, 2> m_posMovAvg;       /**< The moving average of the position over 2 calling cycles. */
 
     /**
      * Default constructor.
@@ -144,8 +142,7 @@ class DrivingState : public IState
         m_topSpeed(0),
         m_lineStatus(LINE_STATUS_FIND_START_LINE),
         m_trackStatus(TRACK_STATUS_ON_TRACK),
-        m_startEndLineDebounce(0),
-        m_posMovAvg()
+        m_startEndLineDebounce(0)
     {
     }
 

diff --git a/lib/APPLineFollower/ParameterSets.cpp b/lib/APPLineFollower/ParameterSets.cpp
@@ -110,7 +110,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets()
         2,          /* Kp Denominator */
         1,          /* Ki Numerator */
         40,         /* Ki Denominator */
-        10,         /* Kd Numerator */
+        40,         /* Kd Numerator */
         1           /* Kd Denominator */
     };
 
@@ -121,7 +121,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets()
         1,         /* Kp Denominator */
         0,         /* Ki Numerator */
         1,         /* Ki Denominator */
-        10,        /* Kd Numerator */
+        40,        /* Kd Numerator */
         1          /* Kd Denominator */
     };
 }