Added auto line crossing failsafe

include/hskylib/subsystems/odometry/odometry_drivebase.h

@@ -17,6 +17,7 @@ class DrivebaseOdometry : public Odometry {
 					  pros::IMU *imu = nullptr, bool async = true);
 
 	void reset() override;
+	void setPose(const pose_t &newPose) override;
 
   protected:
 	void updatePose() override;
@@ -38,6 +39,8 @@ class DrivebaseOdometry : public Odometry {
 	// Getters
 	double getLeftEncoderPosition() const;
 	double getRightEncoderPosition() const;
+
+	double headingOffset_ = 0.0;
 };
 
 #endif

include/hskylib/utils/commands/base_commands.h

@@ -18,7 +18,7 @@ class Command {
 	virtual void initialize() {}
 
 	/**
-	 * Executes the command. This should be a blocking implementation.
+	 * Executes the command. This should be a non-blocking implementation.
 	 */
 	virtual void execute() {}
 

include/hskylib/utils/commands/command_runner.h

@@ -23,11 +23,17 @@ class CommandRunner {
 	 */
 	void run();
 
+	/**
+	 * Stops the CommandRunner and any currently running command.
+	 */
+	void stop();
+
 	~CommandRunner();
 
   private:
 	std::queue<Command *> commands_;
 	Command *currentCommand_;
+	bool isRunning_;
 };
 
 #endif

src/subsystems/odometry/odometry.cpp

@@ -1,4 +1,5 @@
 #include "hskylib/subsystems/odometry/odometry.h"
+#include <hskylib/utils/pose.h>
 
 Odometry::Odometry(bool async) : async_(async) {}
 
@@ -32,7 +33,7 @@ pose_t Odometry::update() {
 
 void Odometry::setPose(const pose_t &newPose) {
 	poseMutex_.take();
-	currentPose_ = newPose;
+	currentPose_ = pose_t(newPose.x, newPose.y, newPose.theta);
 	poseMutex_.give();
 }
 

src/subsystems/odometry/odometry_drivebase.cpp

@@ -1,4 +1,5 @@
 #include "hskylib/subsystems/odometry/odometry_drivebase.h"
+#include "robot_config.hpp"
 
 #include <cmath>
 
@@ -15,8 +16,6 @@ DrivebaseOdometry::DrivebaseOdometry(pros::MotorGroup *left,
 	rightMotorGroup_->set_encoder_units_all(pros::E_MOTOR_ENCODER_DEGREES);
 	leftMotorGroup_->tare_position_all();
 	rightMotorGroup_->tare_position_all();
-
-	init();
 }
 
 void DrivebaseOdometry::reset() {
@@ -28,6 +27,13 @@ void DrivebaseOdometry::reset() {
 	rightMotorGroupLastPosDeg_ = 0;
 }
 
+void DrivebaseOdometry::setPose(const pose_t &newPose) {
+	poseMutex_.take();
+	headingOffset_ = newPose.theta - currentPose_.theta;
+	currentPose_ = pose_t(newPose.x, newPose.y, newPose.theta);
+	poseMutex_.give();
+}
+
 void DrivebaseOdometry::updatePose() {
 	double currLeftPosDeg = getLeftEncoderPosition();
 	double currRightPosDeg = getRightEncoderPosition();
@@ -47,9 +53,9 @@ void DrivebaseOdometry::updatePose() {
 	double newTheta = currentPose_.theta;
 
 	if (imu_) {
-		newTheta = -imu_->get_rotation() * DEG_TO_RAD;
+		newTheta = -imu_->get_rotation() * DEG_TO_RAD + headingOffset_;
 	} else {
-		newTheta += (dR - dL) / specs_.trackWidth;
+		newTheta += (dR - dL) / specs_.trackWidth + headingOffset_;
 	}
 
 	// Normalize to [-pi, pi]
@@ -59,6 +65,14 @@ void DrivebaseOdometry::updatePose() {
 
 	double avgTheta = 0.5 * (currentPose_.theta + newTheta);
 
+	double deltaX = dCenter * std::cos(avgTheta);
+	double deltaY = dCenter * std::sin(avgTheta);
+
+	if (std::isnan(deltaX) || std::isnan(deltaY)) {
+		printf("Warning: NaN detected in odometry update. Skipping pose update.\n");
+		return;
+	}
+
 	currentPose_.x += dCenter * std::cos(avgTheta);
 	currentPose_.y += dCenter * std::sin(avgTheta);
 	currentPose_.theta = newTheta;

src/utils/commands/command_runner.cpp

@@ -1,14 +1,18 @@
 #include "hskylib/utils/commands/command_runner.h"
+#include <hskylib/utils/commands/base_commands.h>
 
 CommandRunner::CommandRunner(std::queue<Command *> commands)
-	: commands_(std::move(commands)), currentCommand_(nullptr) {}
+	: commands_(std::move(commands)), currentCommand_(nullptr) {
+		isRunning_ = false;
+	}
 
 void CommandRunner::run() {
-	while (!commands_.empty()) {
+	isRunning_ = true;
+	while (!commands_.empty() && isRunning_) {
 		currentCommand_ = commands_.front();
 		currentCommand_->initialize();
 
-		while (!currentCommand_->isFinished()) {
+		while (!currentCommand_->isFinished() && isRunning_) {
 			currentCommand_->execute();
 			pros::delay(10);
 		}
@@ -20,13 +24,20 @@ void CommandRunner::run() {
 	}
 }
 
+void CommandRunner::stop() {
+	isRunning_ = false;
+}
+
 CommandRunner::~CommandRunner() {
+	stop();
 	if (currentCommand_) {
 		currentCommand_->end();
 		delete currentCommand_;
+		currentCommand_ = nullptr;
 	}
 	while (!commands_.empty()) {
 		delete commands_.front();
 		commands_.pop();
 	}
 }
+