Merge branch 'main' into SensorFusion/base

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);
 }

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)
     {
     }
 

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 */
     };
 }

lib/ArduinoNative/Arduino.cpp

@@ -240,16 +240,26 @@ extern int main(int argc, char** argv)
          * https://cyberbotics.com/doc/reference/robot#synchronous-versus-asynchronous-controllers
          */
 
-        setup();
-
-        while (true == gSimTime->step())
+        /* Do one single simulation step to force that all sensors will deliver already data.
+         * Otherwise e.g. the position sensor will provide NaN.
+         * This must be done before setup() is called!
+         */
+        if (false == gSimTime->step())
         {
-            keyboard.getPressedButtons();
-            loop();
-            gSocketStream.process();
+            printf("Very first simulation step failed.\n");
+            status = -1;
         }
+        else
+        {
+            setup();
 
-        status = 0;
+            while (true == gSimTime->step())
+            {
+                keyboard.getPressedButtons();
+                loop();
+                gSocketStream.process();
+            }
+        }
     }
 
     return status;

lib/HALSim/Encoders.cpp

@@ -64,11 +64,21 @@ void Encoders::init()
     if (nullptr != m_posSensorLeft)
     {
         m_posSensorLeft->enable(m_simTime.getTimeStep());
+
+        /* If robot reconnets, the position sensor will provide its position.
+         * Ensure that the left encoder will start at 0.
+         */
+        m_lastResetValueLeft = m_posSensorLeft->getValue();
     }
 
     if (nullptr != m_posSensorRight)
     {
         m_posSensorRight->enable(m_simTime.getTimeStep());
+
+        /* If robot reconnets, the position sensor will provide its position.
+         * Ensure that the right encoder will start at 0.
+         */
+        m_lastResetValueRight = m_posSensorRight->getValue();
     }
 }
 
@@ -101,8 +111,8 @@ int16_t Encoders::getCountsRight()
     {
         double currentPos = m_posSensorRight->getValue();
 
-        overflowProtection(m_lastResetValueLeft, currentPos);
-        steps = calculateSteps(m_lastResetValueLeft, currentPos);
+        overflowProtection(m_lastResetValueRight, currentPos);
+        steps = calculateSteps(m_lastResetValueRight, currentPos);
     }
 
     return steps;