From 01b2ec884f1ae917b91dca470b664d56b0fcce72 Mon Sep 17 00:00:00 2001 From: dkt01 Date: Tue, 29 Aug 2023 20:17:04 -0500 Subject: [PATCH] Line following better and started logic for end stop --- src/PlatformApp/PlatformApp.cpp | 5 ++- src/SerialLineSensor/SerialLineSensor.cpp | 12 +++++ src/SerialLineSensor/SerialLineSensor.h | 11 ++++- src/SwervePlatform/SwervePlatform.cpp | 55 ++++++++++++++++------- src/SwervePlatform/SwervePlatform.h | 6 ++- 5 files changed, 67 insertions(+), 22 deletions(-) diff --git a/src/PlatformApp/PlatformApp.cpp b/src/PlatformApp/PlatformApp.cpp index 87fbc28..783cdb2 100644 --- a/src/PlatformApp/PlatformApp.cpp +++ b/src/PlatformApp/PlatformApp.cpp @@ -141,8 +141,9 @@ int main(int /*argc*/, char** /*argv*/) { driveMapLat.map(controllerState.value().Axes.LeftX), driveMapRot.map(controllerState.value().Axes.RightX)); } else if (active) { - swervePlatform.LineFollow( - controllerState.value().Buttons.DUp, controllerState.value().Buttons.DDown, lineSensor.GetArrayStatus()); + swervePlatform.LineFollow(controllerState.value().Buttons.DUp, + controllerState.value().Buttons.DDown, + lineSensor.GetProportionalArrayStatus()); } else { swervePlatform.Stop(); } diff --git a/src/SerialLineSensor/SerialLineSensor.cpp b/src/SerialLineSensor/SerialLineSensor.cpp index 087094d..8a0ac3d 100644 --- a/src/SerialLineSensor/SerialLineSensor.cpp +++ b/src/SerialLineSensor/SerialLineSensor.cpp @@ -57,6 +57,18 @@ SerialLineSensor::~SerialLineSensor() { .rightLineDetected = rawValues.value().right < m_calibrationActivateThreshold}; } +[[nodiscard]] std::optional SerialLineSensor::GetProportionalArrayStatus() const { + auto rawValues = GetRawArrayStatus(); + if (!rawValues) { + return std::nullopt; + } + double range = m_calibrationDeactivateThreshold - m_calibrationActivateThreshold; + return ProportionalArrayStatus{ + .left = std::clamp((m_calibrationDeactivateThreshold - rawValues.value().left) / range, 0.0, 1.0), + .center = std::clamp((m_calibrationDeactivateThreshold - rawValues.value().center) / range, 0.0, 1.0), + .right = std::clamp((m_calibrationDeactivateThreshold - rawValues.value().right) / range, 0.0, 1.0)}; +} + [[nodiscard]] std::optional SerialLineSensor::GetRawArrayStatus() const { std::scoped_lock lock(m_dataMutex); auto timeout = (std::chrono::steady_clock::now() - m_lastUpdateTime) > m_timeout; diff --git a/src/SerialLineSensor/SerialLineSensor.h b/src/SerialLineSensor/SerialLineSensor.h index 7d00e42..b41115c 100644 --- a/src/SerialLineSensor/SerialLineSensor.h +++ b/src/SerialLineSensor/SerialLineSensor.h @@ -21,6 +21,12 @@ struct RawSensorArrayStatus { uint16_t right; }; +struct ProportionalArrayStatus { + double left; ///< 0=no line, 1=full line + double center; ///< 0=no line, 1=full line + double right; ///< 0=no line, 1=full line +}; + class SerialLineSensor { public: SerialLineSensor(const std::string& serialDeviceName, const std::chrono::milliseconds timeout); @@ -33,11 +39,14 @@ class SerialLineSensor { [[nodiscard]] std::optional GetArrayStatus() const; [[nodiscard]] std::optional GetRawArrayStatus() const; + [[nodiscard]] std::optional GetProportionalArrayStatus() const; private: std::string m_serialDeviceName; int m_serialPort; - uint16_t m_calibrationThreshold{1015}; + /// @todo calibration procedure... + uint16_t m_calibrationActivateThreshold{940}; + uint16_t m_calibrationDeactivateThreshold{1000}; std::optional m_currentLeft{std::nullopt}; std::optional m_currentCenter{std::nullopt}; std::optional m_currentRight{std::nullopt}; diff --git a/src/SwervePlatform/SwervePlatform.cpp b/src/SwervePlatform/SwervePlatform.cpp index 30e4827..e5c53ad 100644 --- a/src/SwervePlatform/SwervePlatform.cpp +++ b/src/SwervePlatform/SwervePlatform.cpp @@ -4,6 +4,8 @@ //////////////////////////////////////////////////////////////////////////////////////////////////// #include "SwervePlatform.h" +#include +#include #include "argosLib/general/swerveUtils.h" @@ -27,6 +29,7 @@ void SwervePlatform::SwerveDrive(const double fwVelocity, if (!lineFollow) { m_followDirection = LineFollowDirection::unknown; + m_followState = LineFollowState::normal; } auto moduleStates = RawModuleStates(fwVelocity, latVelocity, rotateVelocity, offset); @@ -93,11 +96,13 @@ 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) { if (!arrayStatus || (!forward && !reverse) || - (!arrayStatus.value().leftLineDetected && !arrayStatus.value().centerLineDetected && - !arrayStatus.value().rightLineDetected)) { + (arrayStatus.value().left < std::numeric_limits::epsilon() && + arrayStatus.value().center < std::numeric_limits::epsilon() && + arrayStatus.value().right < std::numeric_limits::epsilon())) { Stop(); + m_followState = LineFollowState::normal; return; } @@ -105,19 +110,30 @@ void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional 0.5 && arrayStatus.value().center > 0.5 && arrayStatus.value().right > 0.5) { if (desiredFollowDirection == m_followDirection) { // Reached end of line, don't cross - Stop(); + Stop(true); + m_followState = LineFollowState::endStop; + std::cout << "Stop!\n"; return; } else { // Leaving end line. Don't change stored direction because then the platform will stop next loop + m_followState = LineFollowState::endStop; SwerveDrive(forwardSpeed, 0, 0, true); return; } } - m_followDirection = desiredFollowDirection; + if (m_followState == LineFollowState::normal) { + m_followDirection = desiredFollowDirection; + } else if (desiredFollowDirection == m_followDirection) { + m_followState = LineFollowState::pastEnd; + Stop(true); + std::cout << "Stop (past end)!\n"; + return; + } else { + m_followState = LineFollowState::normal; + } frc::Translation2d offset{-2_m, 0_m}; if (desiredFollowDirection == LineFollowDirection::reverse) { @@ -126,24 +142,25 @@ 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)); + } 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)); } if (desiredFollowDirection == LineFollowDirection::reverse) { leftTurnSpeed *= -1.0; } + std::cout << std::setprecision(3) << "l:" << arrayStatus.value().left << " c:" << arrayStatus.value().center + << " r:" << arrayStatus.value().right << " t:" << leftTurnSpeed << '\n'; + SwerveDrive(forwardSpeed, 0, leftTurnSpeed, true, offset); } -void SwervePlatform::Stop() { +void SwervePlatform::Stop(bool active) { for (const auto motor : {&m_motorDriveFrontLeft, &m_motorDriveFrontRight, &m_motorDriveRearRight, @@ -152,7 +169,11 @@ void SwervePlatform::Stop() { &m_motorTurnFrontRight, &m_motorTurnRearRight, &m_motorTurnRearLeft}) { - motor->Set(ctre::phoenix::motorcontrol::TalonFXControlMode::PercentOutput, 0); + if (active) { + motor->Set(ctre::phoenix::motorcontrol::TalonFXControlMode::Velocity, 0); + } else { + motor->Set(ctre::phoenix::motorcontrol::TalonFXControlMode::PercentOutput, 0); + } } } diff --git a/src/SwervePlatform/SwervePlatform.h b/src/SwervePlatform/SwervePlatform.h index d401985..356c9d5 100644 --- a/src/SwervePlatform/SwervePlatform.h +++ b/src/SwervePlatform/SwervePlatform.h @@ -23,6 +23,7 @@ class SwervePlatform { public: enum ModuleIndex { frontLeft, frontRight, rearRight, rearLeft }; enum class LineFollowDirection { forward, reverse, unknown }; + enum class LineFollowState { normal, pastEnd, endStop }; enum class ControlMode { fieldCentric, @@ -65,8 +66,8 @@ class SwervePlatform { const double rotateVelocity, const bool lineFollow = false, frc::Translation2d offset = frc::Translation2d{}); - void LineFollow(bool forward, bool reverse, std::optional arrayStatus); - void Stop(); + void LineFollow(bool forward, bool reverse, std::optional arrayStatus); + void Stop(bool active = false); void Home(const units::degree_t currentAngle); void SetFieldOrientation(const units::degree_t); @@ -107,6 +108,7 @@ class SwervePlatform { ControlMode m_activeControlMode; LineFollowDirection m_followDirection{LineFollowDirection::unknown}; + LineFollowState m_followState{LineFollowState::normal}; }; namespace measureUp {