Line following better and started logic for end stop

dkt01 · Aug 30, 2023 · 01b2ec8 · 01b2ec8
1 parent 7aefe9a
commit 01b2ec8
Show file tree

Hide file tree

Showing 5 changed files with 67 additions and 22 deletions.
diff --git 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
@@ -57,6 +57,18 @@ SerialLineSensor::~SerialLineSensor() {
                            .rightLineDetected = rawValues.value().right < m_calibrationActivateThreshold};
 }
 
+[[nodiscard]] std::optional<ProportionalArrayStatus> 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<RawSensorArrayStatus> 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
@@ -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<SensorArrayStatus> GetArrayStatus() const;
   [[nodiscard]] std::optional<RawSensorArrayStatus> GetRawArrayStatus() const;
+  [[nodiscard]] std::optional<ProportionalArrayStatus> 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<uint16_t> m_currentLeft{std::nullopt};
   std::optional<uint16_t> m_currentCenter{std::nullopt};
   std::optional<uint16_t> m_currentRight{std::nullopt};

diff --git a/src/SwervePlatform/SwervePlatform.cpp b/src/SwervePlatform/SwervePlatform.cpp
@@ -4,6 +4,8 @@
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 #include "SwervePlatform.h"
+#include <iostream>
+#include <iomanip>
 
 #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,31 +96,44 @@ 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<SensorArrayStatus> arrayStatus) {
+void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional<ProportionalArrayStatus> arrayStatus) {
   if (!arrayStatus || (!forward && !reverse) ||
-      (!arrayStatus.value().leftLineDetected && !arrayStatus.value().centerLineDetected &&
-       !arrayStatus.value().rightLineDetected)) {
+      (arrayStatus.value().left < std::numeric_limits<double>::epsilon() &&
+       arrayStatus.value().center < std::numeric_limits<double>::epsilon() &&
+       arrayStatus.value().right < std::numeric_limits<double>::epsilon())) {
     Stop();
+    m_followState = LineFollowState::normal;
     return;
   }
 
   auto desiredFollowDirection = forward ? LineFollowDirection::forward : LineFollowDirection::reverse;
 
   const double forwardSpeed = desiredFollowDirection == LineFollowDirection::forward ? 0.5 : -0.5;
 
-  if (arrayStatus.value().leftLineDetected && arrayStatus.value().centerLineDetected &&
-      arrayStatus.value().rightLineDetected) {
+  if (arrayStatus.value().left > 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<Sensor
 
   double leftTurnSpeed = 0;
 
-  if (arrayStatus.value().leftLineDetected) {
-    leftTurnSpeed = -0.1;
-  } else if (arrayStatus.value().rightLineDetected) {
-    leftTurnSpeed = 0.1;
-  }
-
-  if (arrayStatus.value().centerLineDetected) {
-    leftTurnSpeed *= 0.5;
+  if (arrayStatus.value().left > std::numeric_limits<double>::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<double>::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
@@ -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<SensorArrayStatus> arrayStatus);
-  void Stop();
+  void LineFollow(bool forward, bool reverse, std::optional<ProportionalArrayStatus> 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 {