Arc PID controller

include/VOSS/api.hpp

@@ -7,6 +7,8 @@
 #include "chassis/XDriveChassis.hpp"
 
 #include "controller/AbstractController.hpp"
+#include "controller/ArcPIDController.hpp"
+#include "controller/ArcPIDControllerBuilder.hpp"
 #include "controller/BoomerangController.hpp"
 #include "controller/BoomerangControllerBuilder.hpp"
 #include "controller/PIDController.hpp"
@@ -21,6 +23,7 @@
 
 #include "selector/Selector.hpp"
 
+#include "utils/angle.hpp"
 #include "utils/flags.hpp"
 #include "utils/Point.hpp"
 #include "utils/Pose.hpp"

include/VOSS/controller/ArcPIDController.hpp

@@ -0,0 +1,37 @@
+#pragma once
+
+#include "VOSS/controller/AbstractController.hpp"
+
+namespace voss::controller {
+
+class ArcPIDController : public AbstractController {
+  protected:
+    double linear_kP, linear_kI, linear_kD;
+    double track_width;
+    double exit_error;
+    double min_error;
+    double can_reverse;
+    double settle_time;
+    double prev_t;
+    double slew;
+    double prev_lin_speed;
+
+    double close;
+
+    double prev_lin_err, total_lin_err;
+
+  public:
+    ArcPIDController(std::shared_ptr<localizer::AbstractLocalizer> l);
+
+    double linear_pid(double error);
+
+    chassis::ChassisCommand get_command(bool reverse, bool thru) override;
+    chassis::ChassisCommand get_angular_command(bool reverse,
+                                                bool thru) override;
+
+    void reset() override;
+
+    friend class ArcPIDControllerBuilder;
+};
+
+} // namespace voss::controller

include/VOSS/controller/ArcPIDControllerBuilder.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "VOSS/controller/ArcPIDController.hpp"
+
+namespace voss::controller {
+
+class ArcPIDControllerBuilder {
+  private:
+    ArcPIDController ctrl;
+
+  public:
+    ArcPIDControllerBuilder(std::shared_ptr<localizer::AbstractLocalizer> l);
+
+    static ArcPIDControllerBuilder
+    new_builder(std::shared_ptr<localizer::AbstractLocalizer> l);
+
+    ArcPIDControllerBuilder& with_linear_constants(double kP, double kI,
+                                                   double kD);
+    /*
+     * The track width is measured from your robot.
+     * Due to turning scrub, you want to use a track width a few inches larger
+     * than the real one.
+     */
+    ArcPIDControllerBuilder& with_track_width(double track_width);
+    ArcPIDControllerBuilder& with_exit_error(double error);
+    ArcPIDControllerBuilder& with_min_error(double error);
+    ArcPIDControllerBuilder& with_settle_time(double time);
+    ArcPIDControllerBuilder& with_slew(double slew);
+
+    ArcPIDController build();
+};
+
+} // namespace voss::controller

src/VOSS/controller/ArcPIDController.cpp

@@ -0,0 +1,113 @@
+#include "VOSS/controller/ArcPIDController.hpp"
+
+#include "VOSS/utils/angle.hpp"
+#include <cmath>
+
+namespace voss::controller {
+
+ArcPIDController::ArcPIDController(
+    std::shared_ptr<localizer::AbstractLocalizer> l)
+    : AbstractController(l), prev_lin_err(0.0), total_lin_err(0.0) {
+}
+
+chassis::ChassisCommand ArcPIDController::get_command(bool reverse, bool thru) {
+    Point current_pos = this->l->get_position();
+    double current_angle = this->l->get_orientation_rad();
+
+    // x: current_x, y: current_y
+    // x': target_x, y': target_y
+    // sin: sin(current_angle), cos: cos(current_angle)
+    // line 1: (x, y) + t(-sin, cos)
+    // line 2: ((x+x')/2, (y+y')/2) + s(-(y'-y), (x'-x))
+    // equation 1: t(-sin) + s(y'-y) = (x'-x)/2
+    // equation 2: t(cos) + s(x-x') = (y'-y)/2
+    double a = -sin(current_angle);
+    double b = this->target.y - current_pos.y;
+    double c = (this->target.x - current_pos.x) / 2;
+    double d = cos(current_angle);
+    double e = current_pos.x - this->target.x;
+    double f = (this->target.y - current_pos.y) / 2;
+
+    // apply cramer's rule to solve for t
+    double t;
+    if (fabs(a * e - d * b) < 0.00001) {
+        t = 0.0;
+    } else {
+        t = (c * e - f * b) / (a * e - d * b);
+    }
+
+    double distance_error = sqrt(b * b + e * e);
+    double angle_error = atan2(b, -e) - current_angle;
+
+    if (reverse) {
+        angle_error = atan2(-b, -e) - current_angle;
+    }
+
+    angle_error = voss::norm_delta(angle_error);
+
+    if (distance_error <= exit_error ||
+        (distance_error < min_error && fabs(cos(angle_error)) <= 0.1)) {
+        this->close += 10;
+    } else {
+        this->close = 0;
+    }
+
+    if (close >= settle_time) {
+        return chassis::ChassisCommand{chassis::Stop{}};
+    }
+
+    double lin_speed = thru ? 100.0 : this->linear_pid(distance_error);
+
+    if (distance_error < this->min_error) {
+        this->can_reverse = true;
+        lin_speed *= cos(angle_error);
+        // return chassis::ChassisCommand{chassis::Voltages{lin_speed,
+        // lin_speed}};
+        t = prev_t;
+    } else if (fabs(angle_error) > M_PI_2 && this->can_reverse) {
+        lin_speed = -lin_speed;
+    }
+
+    if (lin_speed > prev_lin_speed + slew) {
+        lin_speed = prev_lin_speed + slew;
+    }
+    lin_speed *= reverse ? -1 : 1;
+
+    double left_speed, right_speed;
+    if (t != 0.0) {
+        // left_speed = (t - track_width / 2) / t * lin_speed;
+        // right_speed = (t + track_width / 2) / t * lin_speed;
+        left_speed = lin_speed * (2 - track_width / t) / 2;
+        right_speed = lin_speed * (2 + track_width / t) / 2;
+    } else {
+        left_speed = lin_speed;
+        right_speed = lin_speed;
+    }
+    prev_t = t;
+    prev_lin_speed = lin_speed;
+    return chassis::ChassisCommand{chassis::Voltages{left_speed, right_speed}};
+}
+
+chassis::ChassisCommand ArcPIDController::get_angular_command(bool reverse,
+                                                              bool thru) {
+    return chassis::ChassisCommand{chassis::Stop{}};
+}
+
+double ArcPIDController::linear_pid(double error) {
+    this->total_lin_err += error;
+
+    double speed = linear_kP * error + linear_kD * (error - prev_lin_err) +
+                   linear_kI * total_lin_err;
+
+    this->prev_lin_err = error;
+
+    return speed;
+}
+
+void ArcPIDController::reset() {
+    this->prev_lin_err = 0.0;
+    this->total_lin_err = 0.0;
+    this->prev_lin_speed = 0.0;
+}
+
+} // namespace voss::controller

src/VOSS/controller/ArcPIDControllerBuilder.cpp

@@ -0,0 +1,57 @@
+#include "VOSS/controller/ArcPIDControllerBuilder.hpp"
+
+namespace voss::controller {
+
+ArcPIDControllerBuilder::ArcPIDControllerBuilder(
+    std::shared_ptr<localizer::AbstractLocalizer> l)
+    : ctrl(l) {
+}
+
+ArcPIDControllerBuilder ArcPIDControllerBuilder::new_builder(
+    std::shared_ptr<localizer::AbstractLocalizer> l) {
+    ArcPIDControllerBuilder builder(l);
+    return builder;
+}
+
+ArcPIDControllerBuilder&
+ArcPIDControllerBuilder::with_linear_constants(double kP, double kI,
+                                               double kD) {
+    this->ctrl.linear_kP = kP;
+    this->ctrl.linear_kI = kI;
+    this->ctrl.linear_kD = kD;
+    return *this;
+}
+
+ArcPIDControllerBuilder&
+ArcPIDControllerBuilder::with_track_width(double track_width) {
+    this->ctrl.track_width = track_width;
+    return *this;
+}
+
+ArcPIDControllerBuilder&
+ArcPIDControllerBuilder::with_exit_error(double error) {
+    this->ctrl.exit_error = error;
+    return *this;
+}
+
+ArcPIDControllerBuilder& ArcPIDControllerBuilder::with_min_error(double error) {
+    this->ctrl.min_error = error;
+    return *this;
+}
+
+ArcPIDControllerBuilder&
+ArcPIDControllerBuilder::with_settle_time(double settle_time) {
+    this->ctrl.settle_time = settle_time;
+    return *this;
+}
+
+ArcPIDControllerBuilder& ArcPIDControllerBuilder::with_slew(double slew) {
+    this->ctrl.slew = slew;
+    return *this;
+}
+
+ArcPIDController ArcPIDControllerBuilder::build() {
+    return this->ctrl;
+}
+
+} // namespace voss::controller

src/main.cpp

@@ -1,6 +1,5 @@
 #include "main.h"
 #include "VOSS/api.hpp"
-#include "VOSS/localizer/ADILocalizerBuilder.hpp"
 /**
  * Runs initialization code. This occurs as soon as the program is started.
  *
@@ -62,11 +61,11 @@ void opcontrol() {
     pros::Controller master(pros::E_CONTROLLER_MASTER);
 
     auto odom = voss::localizer::IMELocalizerBuilder::new_builder()
-                    .with_left_motors({-13, -15, -16})
-                    .with_right_motors({8, 7, 5})
-                    .with_left_right_tpi(19.5) // 19.5
-                    .with_track_width(8.4)     // 3.558
-                    .with_imu(18)
+                    .with_left_motors({-2, -3, -6, -5})
+                    .with_right_motors({11, 12, 19, 20})
+                    .with_left_right_tpi(18.24) // 19.5
+                    .with_track_width(9.75)     // 3.558
+                    .with_imu(13)
                     .build();
 
     odom->begin_localization();
@@ -79,17 +78,30 @@ void opcontrol() {
                    .with_min_error(5)
                    .with_settle_time(200)
                    .build();
+    auto arc = voss::controller::ArcPIDControllerBuilder::new_builder(odom)
+                   .with_linear_constants(6, 0, 50)
+                   .with_track_width(14)
+                   .with_exit_error(1.0)
+                   .with_min_error(5.0)
+                   .with_settle_time(200)
+                   .with_slew(8)
+                   .build();
 
-    voss::chassis::DiffChassis chassis({-13, -15, -16}, {8, 7, 5}, pid, 8);
+    voss::chassis::DiffChassis chassis({-2, -3, -6, -5}, {11, 12, 19, 20}, pid,
+                                       8);
 
     auto [leftM, rightM] = chassis.getMotors();
 
     while (true) {
 
         voss::Pose p = odom->get_pose();
 
+        chassis.arcade(master.get_analog(ANALOG_LEFT_Y),
+                       master.get_analog(ANALOG_RIGHT_X));
+
         if (master.get_digital_new_press(DIGITAL_Y)) {
             odom->set_pose(voss::Pose{0.0, 0.0, 0.0});
+            chassis.move(voss::Point(36, 36), &arc);
         }
 
         pros::lcd::clear_line(1);