Merge pull request #41 from BlueAndi/bugfix/encoders

Bugfix: The right encoder in the simulation used the wrong last posit…
BlueAndi · Nov 18, 2023 · b3f5eb7 · b3f5eb7
2 parents 6ecc118 + a70418e
commit b3f5eb7
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 9 deletions.
diff --git a/lib/ArduinoNative/Arduino.cpp b/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;

diff --git a/lib/HALSim/Encoders.cpp b/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;