From 2ae2e1208332e3a510225dcf249fb054ec0e7d9a Mon Sep 17 00:00:00 2001 From: BlueAndi Date: Sat, 29 Jun 2024 14:47:54 +0200 Subject: [PATCH] LineFollowerSimple is ready now, only parameters need to be optimized. #128 --- .../src/DrivingState.cpp | 85 +++++++++++++++++++ lib/APPLineFollowerSimple/src/DrivingState.h | 25 ++++++ .../src/LineSensorsCalibrationState.cpp | 35 ++++---- .../src/LineSensorsCalibrationState.h | 21 +++-- .../src/ParameterSets.cpp | 8 +- platformio.ini | 7 +- 6 files changed, 149 insertions(+), 32 deletions(-) diff --git a/lib/APPLineFollowerSimple/src/DrivingState.cpp b/lib/APPLineFollowerSimple/src/DrivingState.cpp index 3e1d1dda..8e5c12f9 100644 --- a/lib/APPLineFollowerSimple/src/DrivingState.cpp +++ b/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(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(); diff --git a/lib/APPLineFollowerSimple/src/DrivingState.h b/lib/APPLineFollowerSimple/src/DrivingState.h index ce3e550e..54ec2eaf 100644 --- a/lib/APPLineFollowerSimple/src/DrivingState.h +++ b/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. diff --git a/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.cpp b/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.cpp index 4a498a79..26d17c91 100644 --- a/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.cpp +++ b/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.cpp @@ -34,8 +34,6 @@ *****************************************************************************/ #include "LineSensorsCalibrationState.h" #include -#include -#include #include #include #include "ReadyState.h" @@ -67,10 +65,10 @@ 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"); @@ -78,11 +76,11 @@ void LineSensorsCalibrationState::entry() 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,7 +89,8 @@ void LineSensorsCalibrationState::entry() void LineSensorsCalibrationState::process(StateMachine& sm) { - DifferentialDrive& diffDrive = DifferentialDrive::getInstance(); + IBoard& board = Board::getInstance(); + IMotors& motors = board.getMotors(); switch (m_phase) { @@ -99,7 +98,7 @@ void LineSensorsCalibrationState::process(StateMachine& sm) if (true == m_timer.isTimeout()) { m_phase = PHASE_2_TURN_LEFT; - diffDrive.setLinearSpeed(-m_calibrationSpeed, m_calibrationSpeed); + motors.setSpeeds(-m_calibrationSpeed, m_calibrationSpeed); } break; @@ -107,7 +106,7 @@ void LineSensorsCalibrationState::process(StateMachine& sm) 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; @@ -115,7 +114,7 @@ void LineSensorsCalibrationState::process(StateMachine& sm) 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; @@ -123,7 +122,7 @@ void LineSensorsCalibrationState::process(StateMachine& sm) 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,7 +161,7 @@ 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; } @@ -170,7 +169,7 @@ bool LineSensorsCalibrationState::turnAndCalibrate(int32_t calibAlpha, bool isGr else { /* Is alpha greater or equal than the destination calibration angle? */ - if (calibAlpha <= alpha) + if (calibAlpha <= m_relEncoders.getCountsRight()) { isSuccesful = true; } diff --git a/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.h b/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.h index 3569fe72..9e522315 100644 --- a/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.h +++ b/lib/APPLineFollowerSimple/src/LineSensorsCalibrationState.h @@ -46,6 +46,8 @@ #include #include #include +#include +#include /****************************************************************************** * 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()) { } diff --git a/lib/APPLineFollowerSimple/src/ParameterSets.cpp b/lib/APPLineFollowerSimple/src/ParameterSets.cpp index 050d8501..d350a173 100644 --- a/lib/APPLineFollowerSimple/src/ParameterSets.cpp +++ b/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 */ diff --git a/platformio.ini b/platformio.ini index 4b0cfa42..96211eb8 100644 --- a/platformio.ini +++ b/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