Fix line end logic and smoother line following

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();
         }

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::milliseconds>(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';
         }

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<RawSensorArrayStatus> GetRawArrayStatus() const;
   [[nodiscard]] std::optional<ProportionalArrayStatus> GetProportionalArrayStatus() const;
 
+  [[nodiscard]] RecoveryDirection GetRecoveryDirection();
+
+  [[nodiscard]] bool GetRecoveryActive();
+
  private:
   std::string m_serialDeviceName;
   int m_serialPort;
@@ -52,6 +57,9 @@ class SerialLineSensor {
   std::optional<uint16_t> m_currentRight{std::nullopt};
   std::chrono::time_point<std::chrono::steady_clock> m_lastUpdateTime;
   std::chrono::milliseconds m_timeout{std::chrono::milliseconds{100}};
+  std::chrono::milliseconds m_recoveryTime{std::chrono::milliseconds{1000}};
+  std::chrono::time_point<std::chrono::steady_clock> m_recoveryStartTime;
+  RecoveryDirection m_activeRecoveryDirection{RecoveryDirection::Timeout};
   std::thread m_receiveThread;
   std::atomic<bool> m_runThread{false};
   bool m_connected{false};

src/SwervePlatform/SwervePlatform.cpp

@@ -96,13 +96,15 @@ 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<ProportionalArrayStatus> arrayStatus) {
+void SwervePlatform::LineFollow(bool forward,
+                                bool reverse,
+                                std::optional<ProportionalArrayStatus> arrayStatus,
+                                SerialLineSensor& lineSensor) {
   if (!arrayStatus || (!forward && !reverse) ||
-      (arrayStatus.value().left < std::numeric_limits<double>::epsilon() &&
-       arrayStatus.value().center < std::numeric_limits<double>::epsilon() &&
-       arrayStatus.value().right < std::numeric_limits<double>::epsilon())) {
+      (!lineSensor.GetRecoveryActive() && (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;
   }
 
@@ -131,7 +133,7 @@ void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional<Propor
     Stop(true);
     std::cout << "Stop (past end)!\n";
     return;
-  } else {
+  } else if (m_followState != LineFollowState::pastEnd) {
     m_followState = LineFollowState::normal;
   }
 
@@ -143,11 +145,21 @@ void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional<Propor
   double leftTurnSpeed = 0;
 
   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));
+    leftTurnSpeed = arrayStatus.value().left;
+    if (arrayStatus.value().center <= std::numeric_limits<double>::epsilon() && arrayStatus.value().left < 0.75) {
+      leftTurnSpeed = (2.0 - arrayStatus.value().left);
+    }
+    leftTurnSpeed *= -0.075;
   } 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));
+    leftTurnSpeed = arrayStatus.value().right;
+    if (arrayStatus.value().center <= std::numeric_limits<double>::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) {
+    leftTurnSpeed = 0.15;
   }
 
   if (desiredFollowDirection == LineFollowDirection::reverse) {

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<ProportionalArrayStatus> arrayStatus);
+  void LineFollow(bool forward,
+                  bool reverse,
+                  std::optional<ProportionalArrayStatus> arrayStatus,
+                  SerialLineSensor& lineSensor);
   void Stop(bool active = false);
 
   void Home(const units::degree_t currentAngle);