From 476b80898976b0d7f27a15f5a0260fbd237bc9f6 Mon Sep 17 00:00:00 2001 From: BlueAndi Date: Sat, 18 Nov 2023 14:41:36 +0100 Subject: [PATCH] The derivative part increased for the last 2 parameter sets. Its necessary for the turns. The gap detection reacts now instant on the position value. Otherwise the PID continued and the robot got a kick into a bad direction. Don't constrain for top speed, but for max. speed. This supports better behavior in turns. --- lib/APPLineFollower/DrivingState.cpp | 10 ++++------ lib/APPLineFollower/DrivingState.h | 5 +---- lib/APPLineFollower/ParameterSets.cpp | 4 ++-- 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/lib/APPLineFollower/DrivingState.cpp b/lib/APPLineFollower/DrivingState.cpp index ecb6f275..c64be1ff 100644 --- a/lib/APPLineFollower/DrivingState.cpp +++ b/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); } diff --git a/lib/APPLineFollower/DrivingState.h b/lib/APPLineFollower/DrivingState.h index 45433d88..ad735820 100644 --- a/lib/APPLineFollower/DrivingState.h +++ b/lib/APPLineFollower/DrivingState.h @@ -47,7 +47,6 @@ #include #include #include -#include /****************************************************************************** * 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 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) { } diff --git a/lib/APPLineFollower/ParameterSets.cpp b/lib/APPLineFollower/ParameterSets.cpp index fd36c3e7..49042585 100644 --- a/lib/APPLineFollower/ParameterSets.cpp +++ b/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 */ }; }