From a4db094b85bab5f220350da1cea6570d04654609 Mon Sep 17 00:00:00 2001 From: dkt01 Date: Tue, 5 Sep 2023 22:12:08 -0500 Subject: [PATCH 1/4] Fix state resetting when crossing over line end and returning --- src/SwervePlatform/SwervePlatform.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SwervePlatform/SwervePlatform.cpp b/src/SwervePlatform/SwervePlatform.cpp index e5c53ad..2fa5bb0 100644 --- a/src/SwervePlatform/SwervePlatform.cpp +++ b/src/SwervePlatform/SwervePlatform.cpp @@ -102,7 +102,6 @@ void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional::epsilon() && arrayStatus.value().right < std::numeric_limits::epsilon())) { Stop(); - m_followState = LineFollowState::normal; return; } From 0f56963e101ec51746863e5fd6b14c4fa62f5e35 Mon Sep 17 00:00:00 2001 From: dkt01 Date: Tue, 12 Sep 2023 19:16:35 -0500 Subject: [PATCH 2/4] Add recovery to line follower --- src/PlatformApp/PlatformApp.cpp | 3 ++- src/SerialLineSensor/SerialLineSensor.cpp | 30 +++++++++++++++++++++++ src/SerialLineSensor/SerialLineSensor.h | 8 ++++++ src/SwervePlatform/SwervePlatform.cpp | 23 +++++++++++------ src/SwervePlatform/SwervePlatform.h | 5 +++- 5 files changed, 59 insertions(+), 10 deletions(-) diff --git a/src/PlatformApp/PlatformApp.cpp b/src/PlatformApp/PlatformApp.cpp index 783cdb2..d060cdc 100644 --- a/src/PlatformApp/PlatformApp.cpp +++ b/src/PlatformApp/PlatformApp.cpp @@ -143,7 +143,8 @@ int main(int /*argc*/, char** /*argv*/) { } else if (active) { swervePlatform.LineFollow(controllerState.value().Buttons.DUp, controllerState.value().Buttons.DDown, - lineSensor.GetProportionalArrayStatus()); + lineSensor.GetProportionalArrayStatus(), + lineSensor); } else { swervePlatform.Stop(); } diff --git a/src/SerialLineSensor/SerialLineSensor.cpp b/src/SerialLineSensor/SerialLineSensor.cpp index 8a0ac3d..520134f 100644 --- a/src/SerialLineSensor/SerialLineSensor.cpp +++ b/src/SerialLineSensor/SerialLineSensor.cpp @@ -79,6 +79,25 @@ SerialLineSensor::~SerialLineSensor() { .left = m_currentLeft.value(), .center = m_currentCenter.value(), .right = m_currentRight.value()}; } +[[nodiscard]] SerialLineSensor::RecoveryDirection SerialLineSensor::GetRecoveryDirection() { + std::scoped_lock lock(m_dataMutex); + if (std::chrono::duration_cast(std::chrono::steady_clock::now() - m_recoveryStartTime) > + m_recoveryTime) { + m_activeRecoveryDirection = RecoveryDirection::Timeout; + } + return m_activeRecoveryDirection; +} + +[[nodiscard]] bool SerialLineSensor::GetRecoveryActive() { + switch (GetRecoveryDirection()) { + case RecoveryDirection::Left: + case RecoveryDirection::Right: + return true; + default: + return false; + } +} + void SerialLineSensor::ReceiverThread() { while (m_runThread.load()) { // Connect @@ -169,10 +188,21 @@ void SerialLineSensor::ReceiverThread() { auto rawStates = ParseMessage(std::string_view(buf, nBytes)); if (rawStates) { std::scoped_lock lock(m_dataMutex); + if (m_currentLeft <= m_calibrationDeactivateThreshold && m_currentRight > m_calibrationDeactivateThreshold) { + m_activeRecoveryDirection = RecoveryDirection::Left; + } else if (m_currentRight <= m_calibrationDeactivateThreshold && + m_currentLeft > m_calibrationDeactivateThreshold) { + m_activeRecoveryDirection = RecoveryDirection::Right; + } /// @todo fix left/right in arduino code or something... m_currentLeft = rawStates.value().right; m_currentCenter = rawStates.value().center; m_currentRight = rawStates.value().left; + if (m_currentLeft < m_calibrationDeactivateThreshold || m_currentCenter < m_calibrationDeactivateThreshold || + m_currentRight < m_calibrationDeactivateThreshold) { + m_activeRecoveryDirection = RecoveryDirection::LineDetected; + m_recoveryStartTime = std::chrono::steady_clock::now(); + } m_lastUpdateTime = std::chrono::steady_clock::now(); std::cout << m_currentLeft.value() << ' ' << m_currentCenter.value() << ' ' << m_currentRight.value() << '\n'; } diff --git a/src/SerialLineSensor/SerialLineSensor.h b/src/SerialLineSensor/SerialLineSensor.h index b41115c..1bfa42e 100644 --- a/src/SerialLineSensor/SerialLineSensor.h +++ b/src/SerialLineSensor/SerialLineSensor.h @@ -29,6 +29,7 @@ struct ProportionalArrayStatus { class SerialLineSensor { public: + enum class RecoveryDirection { LineDetected, Left, Right, Timeout }; SerialLineSensor(const std::string& serialDeviceName, const std::chrono::milliseconds timeout); SerialLineSensor(const std::chrono::milliseconds timeout); ~SerialLineSensor(); @@ -41,6 +42,10 @@ class SerialLineSensor { [[nodiscard]] std::optional GetRawArrayStatus() const; [[nodiscard]] std::optional GetProportionalArrayStatus() const; + [[nodiscard]] RecoveryDirection GetRecoveryDirection(); + + [[nodiscard]] bool GetRecoveryActive(); + private: std::string m_serialDeviceName; int m_serialPort; @@ -52,6 +57,9 @@ class SerialLineSensor { std::optional m_currentRight{std::nullopt}; std::chrono::time_point m_lastUpdateTime; std::chrono::milliseconds m_timeout{std::chrono::milliseconds{100}}; + std::chrono::milliseconds m_recoveryTime{std::chrono::milliseconds{1000}}; + std::chrono::time_point m_recoveryStartTime; + RecoveryDirection m_activeRecoveryDirection{RecoveryDirection::Timeout}; std::thread m_receiveThread; std::atomic m_runThread{false}; bool m_connected{false}; diff --git a/src/SwervePlatform/SwervePlatform.cpp b/src/SwervePlatform/SwervePlatform.cpp index 2fa5bb0..d745a4a 100644 --- a/src/SwervePlatform/SwervePlatform.cpp +++ b/src/SwervePlatform/SwervePlatform.cpp @@ -96,11 +96,14 @@ void SwervePlatform::SwerveDrive(const double fwVelocity, measureUp::sensorConversion::swerveRotate::fromAngle(moduleStates.at(ModuleIndex::rearLeft).angle.Degrees())); } -void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional arrayStatus) { +void SwervePlatform::LineFollow(bool forward, + bool reverse, + std::optional arrayStatus, + SerialLineSensor& lineSensor) { if (!arrayStatus || (!forward && !reverse) || - (arrayStatus.value().left < std::numeric_limits::epsilon() && - arrayStatus.value().center < std::numeric_limits::epsilon() && - arrayStatus.value().right < std::numeric_limits::epsilon())) { + (!lineSensor.GetRecoveryActive() && (arrayStatus.value().left < std::numeric_limits::epsilon() && + arrayStatus.value().center < std::numeric_limits::epsilon() && + arrayStatus.value().right < std::numeric_limits::epsilon()))) { Stop(); return; } @@ -142,11 +145,15 @@ void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional std::numeric_limits::epsilon()) { - leftTurnSpeed = -0.05 * (arrayStatus.value().left + - std::clamp(arrayStatus.value().left - arrayStatus.value().center, 0.0, 1.0)); + leftTurnSpeed = -0.075 * (arrayStatus.value().left + + std::clamp(arrayStatus.value().left - arrayStatus.value().center, 0.0, 1.0)); } else if (arrayStatus.value().right > std::numeric_limits::epsilon()) { - leftTurnSpeed = 0.05 * (arrayStatus.value().right + - std::clamp(arrayStatus.value().right - arrayStatus.value().center, 0.0, 1.0)); + leftTurnSpeed = 0.075 * (arrayStatus.value().right + + std::clamp(arrayStatus.value().right - arrayStatus.value().center, 0.0, 1.0)); + } else if (lineSensor.GetRecoveryDirection() == SerialLineSensor::RecoveryDirection::Left) { + leftTurnSpeed = -0.15; + } else if (lineSensor.GetRecoveryDirection() == SerialLineSensor::RecoveryDirection::Right) { + leftTurnSpeed = 0.15; } if (desiredFollowDirection == LineFollowDirection::reverse) { diff --git a/src/SwervePlatform/SwervePlatform.h b/src/SwervePlatform/SwervePlatform.h index 356c9d5..3ac3e5b 100644 --- a/src/SwervePlatform/SwervePlatform.h +++ b/src/SwervePlatform/SwervePlatform.h @@ -66,7 +66,10 @@ class SwervePlatform { const double rotateVelocity, const bool lineFollow = false, frc::Translation2d offset = frc::Translation2d{}); - void LineFollow(bool forward, bool reverse, std::optional arrayStatus); + void LineFollow(bool forward, + bool reverse, + std::optional arrayStatus, + SerialLineSensor& lineSensor); void Stop(bool active = false); void Home(const units::degree_t currentAngle); From 5c205b199a8e141ba1174e56fec101fd459564c3 Mon Sep 17 00:00:00 2001 From: dkt01 Date: Tue, 12 Sep 2023 19:26:53 -0500 Subject: [PATCH 3/4] Fix end stop logic to allow recovery after stop --- src/SwervePlatform/SwervePlatform.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SwervePlatform/SwervePlatform.cpp b/src/SwervePlatform/SwervePlatform.cpp index d745a4a..ed377e5 100644 --- a/src/SwervePlatform/SwervePlatform.cpp +++ b/src/SwervePlatform/SwervePlatform.cpp @@ -133,7 +133,7 @@ void SwervePlatform::LineFollow(bool forward, Stop(true); std::cout << "Stop (past end)!\n"; return; - } else { + } else if (m_followState != LineFollowState::pastEnd) { m_followState = LineFollowState::normal; } From 51701ec4728b6cba2b9db269a99f11bc3f353a6e Mon Sep 17 00:00:00 2001 From: dkt01 Date: Tue, 12 Sep 2023 19:42:14 -0500 Subject: [PATCH 4/4] Kind of proportional line following. Much smoother --- src/SwervePlatform/SwervePlatform.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/SwervePlatform/SwervePlatform.cpp b/src/SwervePlatform/SwervePlatform.cpp index ed377e5..de298cf 100644 --- a/src/SwervePlatform/SwervePlatform.cpp +++ b/src/SwervePlatform/SwervePlatform.cpp @@ -145,11 +145,17 @@ void SwervePlatform::LineFollow(bool forward, double leftTurnSpeed = 0; if (arrayStatus.value().left > std::numeric_limits::epsilon()) { - leftTurnSpeed = -0.075 * (arrayStatus.value().left + - std::clamp(arrayStatus.value().left - arrayStatus.value().center, 0.0, 1.0)); + leftTurnSpeed = arrayStatus.value().left; + if (arrayStatus.value().center <= std::numeric_limits::epsilon() && arrayStatus.value().left < 0.75) { + leftTurnSpeed = (2.0 - arrayStatus.value().left); + } + leftTurnSpeed *= -0.075; } else if (arrayStatus.value().right > std::numeric_limits::epsilon()) { - leftTurnSpeed = 0.075 * (arrayStatus.value().right + - std::clamp(arrayStatus.value().right - arrayStatus.value().center, 0.0, 1.0)); + leftTurnSpeed = arrayStatus.value().right; + if (arrayStatus.value().center <= std::numeric_limits::epsilon() && arrayStatus.value().right < 0.75) { + leftTurnSpeed = (2.0 - arrayStatus.value().right); + } + leftTurnSpeed *= 0.075; } else if (lineSensor.GetRecoveryDirection() == SerialLineSensor::RecoveryDirection::Left) { leftTurnSpeed = -0.15; } else if (lineSensor.GetRecoveryDirection() == SerialLineSensor::RecoveryDirection::Right) {