addded horizontal offset

include/VOSS/localizer/TrackingWheelLocalizer.hpp

@@ -12,11 +12,13 @@ class TrackingWheelLocalizer : public AbstractLocalizer {
   protected:
     std::atomic<double> prev_left_pos, prev_right_pos, prev_middle_pos;
     AtomicPose prev_pose;
+    AtomicPose real_pose;
 
     std::atomic<double> left_right_dist, middle_dist;
     std::unique_ptr<AbstractTrackingWheel> left_tracking_wheel,
         right_tracking_wheel, middle_tracking_wheel;
     std::unique_ptr<pros::IMU> imu;
+    std::atomic<double> offset;
 
   public:
     TrackingWheelLocalizer(std::unique_ptr<AbstractTrackingWheel> left,
@@ -28,6 +30,7 @@ class TrackingWheelLocalizer : public AbstractLocalizer {
     void calibrate() override;
     void set_pose(Pose pose) override;
     void set_pose(double x, double y, double theta) override;
+    void set_horizontal_offset(double offset);
 
     friend class TrackingWheelLocalizerBuilder;
 };

include/VOSS/localizer/TrackingWheelLocalizerBuilder.hpp

@@ -8,6 +8,7 @@ class TrackingWheelLocalizerBuilder {
   private:
     double left_right_dist;
     double middle_dist;
+    double horizontal_offset;
     std::unique_ptr<AbstractTrackingWheel> left_tracking_wheel;
     std::unique_ptr<AbstractTrackingWheel> right_tracking_wheel;
     std::unique_ptr<AbstractTrackingWheel> middle_tracking_wheel;
@@ -38,6 +39,7 @@ class TrackingWheelLocalizerBuilder {
     TrackingWheelLocalizerBuilder& with_track_width(double track_width);
     TrackingWheelLocalizerBuilder& with_middle_dist(double middle_dist);
     TrackingWheelLocalizerBuilder& with_imu(int port);
+    TrackingWheelLocalizerBuilder& with_horizontal_offset(int offset);
 
     std::shared_ptr<TrackingWheelLocalizer> build();
 };

src/VOSS/localizer/TrackingWheelLocalizer.cpp

@@ -35,7 +35,7 @@ void TrackingWheelLocalizer::update() {
     }
     if (imu) {
         pose.theta = -to_radians(imu->get_rotation());
-        delta_angle = pose.theta - prev_pose.theta;
+        delta_angle = real_pose.theta - prev_pose.theta;
     } else {
         delta_angle = (delta_right - delta_left) / (2 * left_right_dist);
         pose.theta += delta_angle;
@@ -44,7 +44,7 @@ void TrackingWheelLocalizer::update() {
     prev_left_pos += delta_left;
     prev_right_pos += delta_right;
     prev_middle_pos += delta_middle;
-    prev_pose = pose;
+    prev_pose = real_pose;
 
     double local_x;
     double local_y;
@@ -67,8 +67,10 @@ void TrackingWheelLocalizer::update() {
     double p = this->pose.theta - delta_angle / 2.0; // global angle
 
     // convert to absolute displacement
-    this->pose.x += cos(p) * local_x - sin(p) * local_y;
-    this->pose.y += sin(p) * local_x + cos(p) * local_y;
+    this->real_pose.x += cos(p) * local_x - sin(p) * local_y;
+    this->real_pose.y += sin(p) * local_x + cos(p) * local_y;
+    this->pose.x = real_pose.x + cos(pose.theta) * offset;
+    this->pose.y = real_pose.y + sin(pose.theta) * offset;
 }
 
 void TrackingWheelLocalizer::calibrate() {
@@ -102,4 +104,9 @@ void TrackingWheelLocalizer::set_pose(double x, double y, double theta) {
     this->set_pose({x, y, theta});
 }
 
+void TrackingWheelLocalizer::set_horizontal_offset(double a) {
+    this->real_pose.x -= (a - this->offset) * cos(pose.theta);
+    this->real_pose.y -= (a - this->offset) * sin(pose.theta);
+    this->offset = a;
+}
 } // namespace voss::localizer

src/VOSS/localizer/TrackingWheelLocalizerBuilder.cpp

@@ -6,7 +6,7 @@
 namespace voss::localizer {
 
 TrackingWheelLocalizerBuilder::TrackingWheelLocalizerBuilder()
-    : left_right_dist(0.0), middle_dist(0.0), left_tracking_wheel(nullptr),
+    : left_right_dist(0.0), middle_dist(0.0), horizontal_offset(0.0), left_tracking_wheel(nullptr),
       right_tracking_wheel(nullptr), middle_tracking_wheel(nullptr),
       imu(nullptr) {
 }
@@ -127,11 +127,19 @@ TrackingWheelLocalizerBuilder::with_imu(int port) {
     return *this;
 }
 
+TrackingWheelLocalizerBuilder&
+TrackingWheelLocalizerBuilder::with_horizontal_offset(int offset) {
+    this->horizontal_offset = offset;
+    return *this; 
+}
+
 std::shared_ptr<TrackingWheelLocalizer> TrackingWheelLocalizerBuilder::build() {
-    return std::make_shared<TrackingWheelLocalizer>(
+    auto result = std::make_shared<TrackingWheelLocalizer>(
         std::move(left_tracking_wheel), std::move(right_tracking_wheel),
         std::move(middle_tracking_wheel), std::move(imu), left_right_dist,
         middle_dist);
+        result->offset = this->horizontal_offset;
+        return result;
 }
 
 } // namespace voss::localizer