Adapted APPSensorFusion to refactored Logging

lib/APPSensorFusion/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;
 
@@ -320,6 +317,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] */
@@ -345,8 +343,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/APPSensorFusion/DrivingState.h

@@ -47,7 +47,6 @@
 #include <IState.h>
 #include <SimpleTimer.h>
 #include <PIDController.h>
-#include <MovAvg.hpp>
 
 /******************************************************************************
  * Macros
@@ -137,11 +136,10 @@ class DrivingState : public IState
     SimpleTimer            m_observationTimer; /**< Observation timer to observe the max. time per challenge. */
     SimpleTimer            m_pidProcessTime;   /**< Timer used for periodically PID processing. */
     PIDController<int16_t> m_pidCtrl;          /**< PID controller, used for driving. */
-    int16_t                m_topSpeed;         /**< Top speed in [steps/s]. It might be lower or equal to the max. speed! */
-    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. */
+    int16_t                m_topSpeed;    /**< Top speed in [steps/s]. It might be lower or equal to the max. speed! */
+    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. */
 
     /**
      * Default constructor.
@@ -153,8 +151,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)
     {
     }
 
@@ -172,7 +169,7 @@ class DrivingState : public IState
     /**
      * Control driving in case the robot is on track.
      *
-     * @param[in] position          Current position on track
+     * @param[in] position           Current position on track
      * @param[in] lineSensorValues   Value of each line sensor
      */
     void processOnTrack(int16_t position, const uint16_t* lineSensorValues);
@@ -181,7 +178,7 @@ class DrivingState : public IState
      * Control driving in case the robot lost the track.
      * It handles the track search algorithm.
      *
-     * @param[in] position          Current position on track
+     * @param[in] position           Current position on track
      * @param[in] lineSensorValues   Value of each line sensor
      */
     void processTrackLost(int16_t position, const uint16_t* lineSensorValues);

lib/APPSensorFusion/ErrorState.h

@@ -27,7 +27,7 @@
 /**
  * @brief  Error state
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup Application
  *
  * @{
@@ -91,21 +91,20 @@ class ErrorState : public IState
 
     /**
      * Set error message, which to show on the display.
-     * 
+     *
      * @param[in] msg   Error message
      */
     void setErrorMsg(const char* msg);
 
 protected:
 private:
-
     /**
      * The error message string size in bytes, which
      * includes the terminating character.
      */
-    static const size_t ERROR_MSG_SIZE  = 20;
+    static const size_t ERROR_MSG_SIZE = 20;
 
-    char    m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */
+    char m_errorMsg[ERROR_MSG_SIZE]; /**< Error message, which to show. */
 
     /**
      * Default constructor.

lib/APPSensorFusion/MotorSpeedCalibrationState.cpp

@@ -65,7 +65,7 @@
 /**
  * Logging source.
  */
-static const char* TAG = "MSCState";
+LOG_TAG("MSCState");
 
 /******************************************************************************
  * Public Methods
@@ -208,8 +208,8 @@ void MotorSpeedCalibrationState::finishCalibration(StateMachine& sm)
     }
     else
     {
-        LOG_DEBUG_VAL(TAG, "Calibrated max. speed (steps/s): ", maxSpeed);
-        LOG_DEBUG_VAL(TAG, "Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM);
+        LOG_INFO_VAL("Calibrated max. speed (steps/s): ", maxSpeed);
+        LOG_INFO_VAL("Calibrated max. speed (mm/s): ", maxSpeed / RobotConstants::ENCODER_STEPS_PER_MM);
 
         sm.setState(&LineSensorsCalibrationState::getInstance());
     }

lib/APPSensorFusion/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/APPSensorFusion/ParameterSets.h

@@ -27,7 +27,7 @@
 /**
  * @brief  Parameter state
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup Application
  *
  * @{

lib/APPSensorFusion/ReadyState.cpp

@@ -62,7 +62,7 @@
 /**
  * Logging source.
  */
-static const char* TAG = "RState";
+LOG_TAG("RState");
 
 /******************************************************************************
  * Public Methods
@@ -93,9 +93,9 @@ void ReadyState::process(StateMachine& sm)
         uint8_t         index        = 0;
         int16_t         position     = lineSensors.readLine();
         const uint16_t* sensorValues = lineSensors.getSensorValues();
-        char valueStr[10];
+        char            valueStr[10];
 
-        LOG_DEBUG_HEAD(TAG);
+        LOG_DEBUG_HEAD();
 
         /* Print line sensor value on console for debug purposes. */
         for (index = 0; index < lineSensors.getNumLineSensors(); ++index)
@@ -106,12 +106,12 @@ void ReadyState::process(StateMachine& sm)
             }
 
             Util::uintToStr(valueStr, sizeof(valueStr), sensorValues[index]);
-            
+
             LOG_DEBUG_MSG(valueStr);
         }
 
         LOG_DEBUG_MSG(" -> ");
-        
+
         Util::intToStr(valueStr, sizeof(valueStr), position);
         LOG_DEBUG_MSG(valueStr);
 

lib/APPSensorFusion/ReadyState.h

@@ -27,7 +27,7 @@
 /**
  * @brief  Ready state
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup Application
  *
  * @{

lib/APPSensorFusion/ReleaseTrackState.h

@@ -27,7 +27,7 @@
 /**
  * @brief  Release track state
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup Application
  *
  * @{

lib/APPSensorFusion/StartupState.h

@@ -27,7 +27,7 @@
 /**
  * @brief  Startup state
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup Application
  *
  * @{