Refactor Pose and AtomicPose

include/VOSS/chassis/AbstractChassis.hpp

@@ -36,12 +36,9 @@ class AbstractChassis {
     virtual bool execute(DiffChassisCommand cmd, double max) = 0;
     virtual void set_brake_mode(pros::motor_brake_mode_e mode) = 0;
 
-    void move(Point target, controller_ptr controller, double max = 100.0,
-              voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
     void move(Pose target, controller_ptr controller, double max = 100.0,
               voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
-    void move(Point target, double max = 100.0,
-              voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
+
     void move(Pose target, double max = 100.0,
               voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
 

include/VOSS/localizer/AbstractLocalizer.hpp

@@ -10,7 +10,7 @@ namespace voss::localizer {
 class AbstractLocalizer {
   protected:
     pros::Mutex mtx;
-    Pose pose;
+    AtomicPose pose;
 
   public:
     AbstractLocalizer();

include/VOSS/localizer/IMELocalizer.hpp

@@ -12,7 +12,7 @@ class IMELocalizer : public AbstractLocalizer {
 
   private:
     double prev_left_pos, prev_right_pos, prev_middle_pos;
-    Pose prev_pose;
+    AtomicPose prev_pose;
 
     double left_right_tpi, middle_tpi;
     double track_width;

include/VOSS/utils/Pose.hpp

@@ -4,10 +4,11 @@
 
 namespace voss {
 
+struct AtomicPose;
 struct Pose {
     double x;
     double y;
-    double theta;
+    std::optional<double> theta = std::nullopt;
 };
 
 struct AtomicPose {
@@ -18,7 +19,17 @@ struct AtomicPose {
     void operator=(const Pose& pose) {
         x = pose.x;
         y = pose.y;
-        theta = pose.theta;
+        if (pose.theta.has_value()) {
+            theta = pose.theta.value();
+        } else {
+            theta = NAN;
+        }
+    }
+
+    void operator=(const AtomicPose& pose) {
+        x = pose.x.load();
+        y = pose.y.load();
+        theta = pose.theta.load();
     }
 };
 

src/VOSS/chassis/AbstractChassis.cpp

@@ -65,23 +65,11 @@ void AbstractChassis::turn_task(controller_ptr controller, double max,
     this->task->join();
 }
 
-// Overloaded constructors move functions to allow for different parameters
-void AbstractChassis::move(Point target, double max, voss::Flags flags,
-                           double exitTime) {
-    this->move(target, this->default_controller, max, flags, exitTime);
-}
-
 void AbstractChassis::move(Pose target, double max, voss::Flags flags,
                            double exitTime) {
     this->move(target, this->default_controller, max, flags, exitTime);
 }
 
-void AbstractChassis::move(Point target, controller_ptr controller, double max,
-                           voss::Flags flags, double exitTime) {
-    Pose pose_target = Pose{target.x, target.y, 361};
-    this->move(pose_target, std::move(controller), max, flags, exitTime);
-}
-
 void AbstractChassis::move(Pose target, controller_ptr controller, double max,
                            voss::Flags flags, double exitTime) {
     while (this->task_running) {
@@ -129,7 +117,7 @@ void AbstractChassis::turn_to(Point target, controller_ptr controller,
     }
     this->task_running = true;
 
-    controller->set_target({target.x, target.y, 361},
+    controller->set_target({target.x, target.y, std::nullopt},
                            flags & voss::Flags::RELATIVE);
 
     this->turn_task(std::move(controller), max, flags, direction, exitTime);

src/VOSS/controller/AbstractController.cpp

@@ -12,12 +12,19 @@ AbstractController::AbstractController(
 // Set desired postion with x, y, and heading
 // Relative target position WIP
 void AbstractController::set_target(Pose target, bool relative) {
+    if (target.theta.has_value()) {
+        target.theta = voss::to_radians(*target.theta);
+    }
     if (relative) {
         Point p = l->get_position();         // robot position
         double h = l->get_orientation_rad(); // robot heading in radians
         double x_new = p.x + target.x * cos(h) - target.y * sin(h);
         double y_new = p.y + target.x * sin(h) + target.y * cos(h);
-        this->target = Pose{x_new, y_new, 361};
+        if (target.theta.has_value()) {
+            this->target = Pose{x_new, y_new, target.theta.value() + h};
+        } else {
+            this->target = Pose{x_new, y_new, std::nullopt};
+        }
     } else {
         this->target = target;
     }

src/VOSS/controller/BoomerangController.cpp

@@ -14,10 +14,16 @@ BoomerangController::BoomerangController(
 
 chassis::DiffChassisCommand BoomerangController::get_command(bool reverse,
                                                              bool thru) {
+
+    if (!target.theta.has_value()) {
+        return chassis::DiffChassisCommand{chassis::Stop{}};
+    }
+
     Point current_pos = this->l->get_position();
     double k = this->min_vel / 100;
-    Point virtualTarget = {target.x + k * cos(voss::to_radians(target.theta)),
-                           target.y + k * sin(voss::to_radians(target.theta))};
+    Point virtualTarget = {
+        target.x + k * cos(target.theta.value()),
+        target.y + k * sin(target.theta.value())};
     double dx, dy, distance_error, at;
     int dir = reverse ? -1 : 1;
     Pose trueTarget;
@@ -31,7 +37,7 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse,
         trueTarget = this->target;
     };
     distance_error = sqrt(dx * dx + dy * dy);
-    at = voss::to_radians(target.theta);
+    at = voss::to_radians(target.theta.value());
 
     this->carrotPoint = {trueTarget.x - distance_error * cos(at) * lead_pct,
                          trueTarget.y - distance_error * sin(at) * lead_pct,
@@ -41,7 +47,7 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse,
     double current_angle =
         this->l->get_orientation_rad() + (reverse ? M_PI : 0);
     bool chainedExecutable = false;
-    bool noPose = this->target.theta == 361;
+    bool noPose = !this->target.theta.has_value();
     double angle_error;
     double m = fabs((current_pos.y - target.y) / (current_pos.x - target.x));
 
@@ -57,9 +63,9 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse,
         virtualTarget.y*/
         thru &&
         (current_pos.y - virtualTarget.y) *
-                cos(voss::to_radians(target.theta) - M_PI_2) <
+                cos(target.theta.value() - M_PI_2) <
             (current_pos.x - virtualTarget.x) *
-                sin(voss::to_radians(target.theta) - M_PI_2) &&
+                sin(target.theta.value() - M_PI_2) &&
         (pow((current_pos.y - virtualTarget.y), 2) +
          pow((current_pos.x - virtualTarget.x), 2)) <
             pow(this->min_error * 2, 2)) {
@@ -107,7 +113,7 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse,
                            // spinning
         } else {
             // turn to face the finale pose angle if executing a pose movement
-            double poseError = (target.theta * M_PI / 180) - current_angle;
+            double poseError = target.theta.value() - current_angle;
             while (fabs(poseError) > M_PI)
                 poseError -= 2 * M_PI * poseError / fabs(poseError);
             ang_speed = angular_pid(poseError);

src/VOSS/controller/PIDController.cpp

@@ -28,7 +28,7 @@ chassis::DiffChassisCommand PIDController::get_command(bool reverse,
     Point current_pos = this->l->get_position();
     double current_angle = this->l->get_orientation_rad();
     bool chainedExecutable = false;
-    bool noPose = this->target.theta == 361;
+    bool noPose = !this->target.theta.has_value();
 
     double dx = target.x - current_pos.x;
     double dy = target.y - current_pos.y;
@@ -89,7 +89,7 @@ chassis::DiffChassisCommand PIDController::get_command(bool reverse,
                            // spinning
         } else {
             // turn to face the finale pose angle if executing a pose movement
-            double poseError = (target.theta * M_PI / 180) - current_angle;
+            double poseError = target.theta.value() - current_angle;
             while (fabs(poseError) > M_PI)
                 poseError -= 2 * M_PI * poseError / fabs(poseError);
             ang_speed = angular_pid(poseError);
@@ -130,7 +130,7 @@ PIDController::get_angular_command(bool reverse, bool thru,
     counter += 10;
     double current_angle = this->l->get_orientation_rad();
     double target_angle = 0;
-    if (this->target.theta == 361) {
+    if (!this->target.theta.has_value()) {
         Point current_pos = this->l->get_position();
         double dx = this->target.x - current_pos.x;
         double dy = this->target.y - current_pos.y;

src/VOSS/controller/SwingController.cpp

@@ -28,7 +28,7 @@ SwingController::get_angular_command(bool reverse, bool thru,
     counter += 10;
     double current_angle = this->l->get_orientation_rad();
     double target_angle = 0;
-    if (this->target.theta == 361) {
+    if (!this->target.theta.has_value()) {
         Point current_pos = this->l->get_position();
         double dx = this->target.x - current_pos.x;
         double dy = this->target.y - current_pos.y;

src/VOSS/localizer/ADILocalizer.cpp

@@ -86,7 +86,7 @@ void ADILocalizer::calibrate() {
             pros::delay(10);
         }
     }
-    this->pose = {0.0, 0.0, 0.0};
+    this->pose = voss::AtomicPose{0.0, 0.0, 0.0};
 }
 
 // Calculates the current position of the robot
@@ -149,8 +149,8 @@ void ADILocalizer::update() {
 
 void ADILocalizer::set_pose(Pose pose) {
     this->AbstractLocalizer::set_pose(pose);
-    if (this->imu) {
-        this->imu->set_rotation(-pose.theta);
+    if (this->imu && pose.theta.has_value()) {
+        this->imu->set_rotation(-pose.theta.value());
     }
 }
 

src/VOSS/localizer/AbstractLocalizer.cpp

@@ -7,7 +7,7 @@ namespace voss::localizer {
 // Creating the localiozer object with a starting pose of x = 0, y = 0, and
 // heading = 0
 AbstractLocalizer::AbstractLocalizer() {
-    this->pose = {0.0, 0.0, 0.0};
+    this->pose = AtomicPose{0.0, 0.0, 0.0};
 }
 
 // Starting the localization task
@@ -29,12 +29,20 @@ void AbstractLocalizer::begin_localization() {
 // pose while keeing the values safe with mutex
 void AbstractLocalizer::set_pose(Pose pose) {
     std::unique_lock<pros::Mutex> lock(this->mtx);
-    this->pose = {pose.x, pose.y, to_radians(pose.theta)};
+    if (pose.theta.has_value()) {
+        this->pose =
+            AtomicPose{pose.x, pose.y, voss::to_radians(pose.theta.value())};
+    } else {
+        double h = this->pose.theta;
+        this->pose = AtomicPose{pose.x, pose.y, h};
+    }
 }
 
 Pose AbstractLocalizer::get_pose() {
     std::unique_lock<pros::Mutex> lock(this->mtx);
-    Pose ret = this->pose;
+    Pose ret = {this->pose.x.load(), this->pose.y.load(),
+                this->pose.theta.load()};
+
     return ret;
 }
 

src/VOSS/localizer/IMELocalizer.cpp

@@ -95,7 +95,7 @@ void IMELocalizer::calibrate() {
     if (horizontal_motors) {
         horizontal_motors->tare_position();
     }
-    this->pose = {0.0, 0.0, 0.0};
+    this->pose = voss::AtomicPose{0.0, 0.0, 0.0};
 }
 // Calculates the current position of the robot
 // Uses the change in value of the encoders to calculate the change in position
@@ -154,8 +154,8 @@ void IMELocalizer::update() {
 
 void IMELocalizer::set_pose(Pose pose) {
     this->AbstractLocalizer::set_pose(pose);
-    if (this->imu) {
-        this->imu->set_rotation(-pose.theta);
+    if (this->imu && pose.theta.has_value()) {
+        this->imu->set_rotation(-pose.theta.value());
     }
 }
 

src/VOSS/localizer/TrackingWheelLocalizer.cpp

@@ -81,14 +81,14 @@ void TrackingWheelLocalizer::calibrate() {
     if (imu) {
         imu->reset(true);
     }
-    this->pose = {0.0, 0.0, 0.0};
+    this->pose = AtomicPose{0.0, 0.0, 0.0};
 }
 
 void TrackingWheelLocalizer::set_pose(Pose pose) {
     this->AbstractLocalizer::set_pose(pose);
     this->prev_pose = this->pose;
-    if (this->imu) {
-        this->imu->set_rotation(-pose.theta);
+    if (this->imu && pose.theta.has_value()) {
+        this->imu->set_rotation(-pose.theta.value());
     }
 }