LineFollowerSimple is ready now, only parameters need to be optimized.

#128

lib/APPLineFollowerSimple/src/DrivingState.cpp

@@ -120,6 +120,8 @@ void DrivingState::process(StateMachine& sm)
     int16_t         position         = lineSensors.readLine();
     const uint16_t* lineSensorValues = lineSensors.getSensorValues();
     uint8_t         numLineSensors   = lineSensors.getNumLineSensors();
+    int16_t         position3        = 0;
+    bool            isPosition3Valid = calcPosition3(position3, lineSensorValues, numLineSensors);
     bool            isTrackLost      = isNoLineDetected(lineSensorValues, numLineSensors);
 
     /* ========================================================================
@@ -128,6 +130,12 @@ void DrivingState::process(StateMachine& sm)
      */
     nextTrackStatus = evaluateSituation(lineSensorValues, numLineSensors, position, isTrackLost);
 
+    /* ========================================================================
+     * Initiate measures depended on the situation.
+     * ========================================================================
+     */
+    processSituation(position, allowNegativeMotorSpeed, nextTrackStatus, position3);
+
     /* If the tracks status changes, the PID integral and derivative part
      * will be reset to provide a smooth reaction.
      */
@@ -274,6 +282,35 @@ DrivingState::DrivingState() :
 {
 }
 
+bool DrivingState::calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const
+{
+    const int32_t WEIGHT      = SENSOR_VALUE_MAX;
+    bool          isValid     = true;
+    int32_t       numerator   = 0U;
+    int32_t       denominator = 0U;
+    int32_t       idxBegin    = 1;
+    int32_t       idxEnd      = length - 1;
+
+    for (int32_t idx = idxBegin; idx < idxEnd; ++idx)
+    {
+        int32_t sensorValue = static_cast<int32_t>(lineSensorValues[idx]);
+
+        numerator += idx * WEIGHT * sensorValue;
+        denominator += sensorValue;
+    }
+
+    if (0 == denominator)
+    {
+        isValid = false;
+    }
+    else
+    {
+        position = numerator / denominator;
+    }
+
+    return isValid;
+}
+
 DrivingState::TrackStatus DrivingState::evaluateSituation(const uint16_t* lineSensorValues, uint8_t length,
                                                           int16_t position, bool isTrackLost) const
 {
@@ -374,6 +411,54 @@ bool DrivingState::isNoLineDetected(const uint16_t* lineSensorValues, uint8_t le
     return isDetected;
 }
 
+void DrivingState::processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3)
+{
+    switch (trackStatus)
+    {
+    case TRACK_STATUS_NORMAL:
+        /* The position is used by default to follow the line. */
+        break;
+
+    case TRACK_STATUS_START_STOP_LINE:
+        /* Use the inner sensors only for driving to avoid jerky movements, caused
+         * by the most left or right sensor.
+         */
+        position = position3;
+
+        /* Avoid that the robot turns in place. */
+        allowNegativeMotorSpeed = false;
+        break;
+
+    case TRACK_STATUS_TRACK_LOST_BY_GAP:
+        /* Overwrite the positin to drive straight forward in hope to see the
+         * line again.
+         */
+        position = POSITION_SET_POINT;
+
+        /* Avoid that the robots turns. */
+        allowNegativeMotorSpeed = false;
+        break;
+
+    case TRACK_STATUS_TRACK_LOST_BY_MANOEUVRE:
+        /* If the track is lost by a robot manoeuvre and not becase the track
+         * has a gap, the last position given by most left or right sensor
+         * will be automatically used to find the track again.
+         *
+         * See ILineSensors::readLine() description regarding the behaviour in
+         * case the line is not detected anymore.
+         */
+        break;
+
+    case TRACK_STATUS_FINISHED:
+        /* Nothing to do. */
+        break;
+
+    default:
+        /* Should never happen. */
+        break;
+    }
+}
+
 void DrivingState::adaptDriving(int16_t position, bool allowNegativeMotorSpeed)
 {
     IBoard&       board           = Board::getInstance();

lib/APPLineFollowerSimple/src/DrivingState.h

@@ -206,6 +206,18 @@ class DrivingState : public IState
      */
     DrivingState& operator=(const DrivingState& state);
 
+    /**
+     * Calculate the position with the inner 3 line sensors.
+     *
+     * @param[out]  position            The position result.
+     * @param[in]   lineSensorValues    Array of line sensor values.
+     * @param[in]   length              Array length.
+     *
+     * @return If successful, it will return true otherwise false.
+     */
+    bool calcPosition3(int16_t& position, const uint16_t* lineSensorValues, uint8_t length) const;
+
+
     /**
      * Evaluate the situation by line sensor values and position and determine
      * the track status. The result influences the measures to keep track on
@@ -241,6 +253,19 @@ class DrivingState : public IState
      */
     bool isNoLineDetected(const uint16_t* lineSensorValues, uint8_t length) const;
 
+    /**
+     * Process the situation and decide which measures to take.
+     * Measures will influence the position or whether its allowed to have
+     * a negative motor speed.
+     *
+     * @param[in, out]  position                    The position calculated with all sensors in digits.
+     * @param[out]      allowNegativeMotorSpeed     Allow negative motor speed or not.
+     * @param[in]       trackStatus                 The evaluated track status.
+     * @param[in]       position3                   The position calculated with the inner sensors in digits.
+     */
+    void processSituation(int16_t& position, bool& allowNegativeMotorSpeed, TrackStatus trackStatus, int16_t position3);
+
+
     /**
      * Adapt driving by using a PID algorithm, depended on the position
      * input.

lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.cpp

@@ -34,8 +34,6 @@
  *****************************************************************************/
 #include "LineSensorsCalibrationState.h"
 #include <Board.h>
-#include <DifferentialDrive.h>
-#include <Odometry.h>
 #include <StateMachine.h>
 #include <Util.h>
 #include "ReadyState.h"
@@ -67,22 +65,22 @@
 
 void LineSensorsCalibrationState::entry()
 {
-    IDisplay&          display     = Board::getInstance().getDisplay();
-    DifferentialDrive& diffDrive   = DifferentialDrive::getInstance();
-    Odometry&          odometry    = Odometry::getInstance();
-    ILineSensors&      lineSensors = Board::getInstance().getLineSensors();
+    IBoard&       board       = Board::getInstance();
+    IMotors&      motors      = board.getMotors();
+    IDisplay&     display     = board.getDisplay();
+    ILineSensors& lineSensors = board.getLineSensors();
 
     display.clear();
     display.print("Run");
     display.gotoXY(0, 1);
     display.print("LCAL");
 
     /* Prepare calibration drive. */
-    m_calibrationSpeed = diffDrive.getMaxMotorSpeed() / 4;
-    m_orientation      = odometry.getOrientation();
+    m_calibrationSpeed = motors.getMaxSpeed() / 4;
 
     /* Mandatory for each new calibration. */
     lineSensors.resetCalibration();
+    m_relEncoders.clear();
 
     /* Wait some time, before starting the calibration drive. */
     m_phase = PHASE_1_WAIT;
@@ -91,39 +89,40 @@ void LineSensorsCalibrationState::entry()
 
 void LineSensorsCalibrationState::process(StateMachine& sm)
 {
-    DifferentialDrive& diffDrive = DifferentialDrive::getInstance();
+    IBoard&  board  = Board::getInstance();
+    IMotors& motors = board.getMotors();
 
     switch (m_phase)
     {
     case PHASE_1_WAIT:
         if (true == m_timer.isTimeout())
         {
             m_phase = PHASE_2_TURN_LEFT;
-            diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed);
+            motors.setSpeeds(-m_calibrationSpeed, m_calibrationSpeed);
         }
         break;
 
     case PHASE_2_TURN_LEFT:
         if (true == turnAndCalibrate(CALIB_ANGLE, true))
         {
             m_phase = PHASE_3_TURN_RIGHT;
-            diffDrive.setLinearSpeed(m_calibrationSpeed, -m_calibrationSpeed);
+            motors.setSpeeds(m_calibrationSpeed, -m_calibrationSpeed);
         }
         break;
 
     case PHASE_3_TURN_RIGHT:
         if (true == turnAndCalibrate(-CALIB_ANGLE, false))
         {
             m_phase = PHASE_4_TURN_ORIG;
-            diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed);
+            motors.setSpeeds(-m_calibrationSpeed, m_calibrationSpeed);
         }
         break;
 
     case PHASE_4_TURN_ORIG:
         if (true == turnAndCalibrate(0, true))
         {
             m_phase = PHASE_5_FINISHED;
-            diffDrive.setLinearSpeed(0, 0);
+            motors.setSpeeds(0, 0);
             finishCalibration(sm);
         }
         break;
@@ -150,9 +149,9 @@ void LineSensorsCalibrationState::exit()
 
 bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGreaterEqual)
 {
-    ILineSensors& lineSensors = Board::getInstance().getLineSensors();
-    Odometry&     odometry    = Odometry::getInstance();
-    int32_t       alpha       = odometry.getOrientation() - m_orientation; /* [mrad] */
+    IBoard&       board       = Board::getInstance();
+    IMotors&      motors      = board.getMotors();
+    ILineSensors& lineSensors = board.getLineSensors();
     bool          isSuccesful = false;
 
     /* Continously calibrate the line sensors. */
@@ -162,15 +161,15 @@ bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGr
     if (false == isGreaterEqual)
     {
         /* Is alpha lower or equal than the destination calibration angle? */
-        if (calibAlpha >= alpha)
+        if (calibAlpha >= m_relEncoders.getCountsRight())
         {
             isSuccesful = true;
         }
     }
     else
     {
         /* Is alpha greater or equal than the destination calibration angle? */
-        if (calibAlpha <= alpha)
+        if (calibAlpha <= m_relEncoders.getCountsRight())
         {
             isSuccesful = true;
         }

lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.h

@@ -46,6 +46,8 @@
 #include <IState.h>
 #include <SimpleTimer.h>
 #include <FPMath.h>
+#include <RelativeEncoders.h>
+#include <RobotConstants.h>
 
 /******************************************************************************
  * Macros
@@ -108,19 +110,24 @@ class LineSensorsCalibrationState : public IState
     static const uint32_t WAIT_TIME = 1000;
 
     /**
-     * Calibration turn angle in mrad (corresponds to 72°).
+     * Calibration turn angle in encoder steps (corresponds to 72°).
      */
-    static const int32_t CALIB_ANGLE = (FP_2PI() / 5);
+    static const int32_t CALIB_ANGLE =
+        (((72 * 3 * RobotConstants::WHEEL_BASE) / 360) * RobotConstants::ENCODER_STEPS_PER_M) / 1000;
 
-    SimpleTimer m_timer;            /**< Timer used to wait, until the calibration drive starts. */
-    Phase       m_phase;            /**< Current calibration phase */
-    int16_t     m_calibrationSpeed; /**< Speed in steps/s for the calibration drive. */
-    int32_t     m_orientation;      /**< Orientation at the begin of the calibration in [mrad]. */
+    SimpleTimer      m_timer;            /**< Timer used to wait, until the calibration drive starts. */
+    Phase            m_phase;            /**< Current calibration phase */
+    int16_t          m_calibrationSpeed; /**< Speed in digits for the calibration drive. */
+    RelativeEncoders m_relEncoders;      /**< Relative encoders to measure turn angle. */
 
     /**
      * Default constructor.
      */
-    LineSensorsCalibrationState() : m_timer(), m_phase(PHASE_1_WAIT), m_calibrationSpeed(0), m_orientation(0)
+    LineSensorsCalibrationState() :
+        m_timer(),
+        m_phase(PHASE_1_WAIT),
+        m_calibrationSpeed(0),
+        m_relEncoders(Board::getInstance().getEncoders())
     {
     }
 

lib/APPLineFollowerSimple/src/ParameterSets.cpp

@@ -94,7 +94,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets()
 {
     m_parSets[0] = {
         "PD VF", /* Name - VF: very fast */
-        4000,    /* Top speed in steps/s */
+        400,     /* Top speed in steps/s */
         4,       /* Kp Numerator */
         1,       /* Kp Denominator */
         0,       /* Ki Numerator */
@@ -105,7 +105,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets()
 
     m_parSets[1] = {
         "PD F", /* Name - F: fast */
-        3000,   /* Top speed in steps/s */
+        300,    /* Top speed in steps/s */
         2,      /* Kp Numerator */
         1,      /* Kp Denominator */
         0,      /* Ki Numerator */
@@ -116,7 +116,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets()
 
     m_parSets[2] = {
         "PD S", /* Name - S: slow */
-        2000,   /* Top speed in steps/s */
+        200,    /* Top speed in steps/s */
         2,      /* Kp Numerator */
         1,      /* Kp Denominator */
         0,      /* Ki Numerator */
@@ -127,7 +127,7 @@ ParameterSets::ParameterSets() : m_currentSetId(0), m_parSets()
 
     m_parSets[3] = {
         "PD VS", /* Name - VS: very slow */
-        1000,    /* Top speed in steps/s */
+        100,     /* Top speed in steps/s */
         3,       /* Kp Numerator */
         2,       /* Kp Denominator */
         0,       /* Ki Numerator */

platformio.ini

@@ -17,10 +17,11 @@
 ;default_envs = CalibSim
 ;default_envs = ConvoyLeaderTarget
 ;default_envs = ConvoyLeaderSim
-; default_envs = ConvoyFollowerSim
-; default_envs = ConvoyFollowerTarget
+;default_envs = ConvoyFollowerSim
+;default_envs = ConvoyFollowerTarget
 ;default_envs = LineFollowerTarget
-default_envs = LineFollowerSim
+;default_envs = LineFollowerSim
+default_envs = LineFollowerSimpleSim
 ;default_envs = RemoteControlTarget
 ;default_envs = RemoteControlSim
 ;default_envs = SensorFusionTarget