ArcPIDController Optimization (#89)

purduesigbots · Mar 23, 2024 · 51a9b87 · 51a9b87
1 parent 1583605
commit 51a9b87
Show file tree

Hide file tree

Showing 6 changed files with 45 additions and 31 deletions.
diff --git a/include/VOSS/controller/ArcPIDController.hpp b/include/VOSS/controller/ArcPIDController.hpp
@@ -10,9 +10,12 @@ class ArcPIDController : public AbstractController {
   protected:
     std::shared_ptr<ArcPIDController> p;
     utils::PID linear_pid;
+    utils::PID angular_pid;
     double track_width;
     double min_error;
     double can_reverse;
+    double arc_radius;
+    Point arc_center;
     double prev_t;
     double slew;
     double prev_lin_speed;

diff --git a/include/VOSS/controller/ArcPIDControllerBuilder.hpp b/include/VOSS/controller/ArcPIDControllerBuilder.hpp
@@ -18,6 +18,7 @@ class ArcPIDControllerBuilder {
 
     ArcPIDControllerBuilder& with_linear_constants(double kP, double kI,
                                                    double kD);
+    ArcPIDControllerBuilder& with_angular_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

diff --git a/src/VOSS/controller/ArcPIDController.cpp b/src/VOSS/controller/ArcPIDController.cpp
@@ -39,11 +39,20 @@ ArcPIDController::get_command(bool reverse, bool thru,
         t = (c * e - f * b) / (a * e - d * b);
     }
 
+    if (std::isnan(arc_radius)) {
+        arc_radius = t;
+        arc_center = {
+            current_pos.x + t * a,
+            current_pos.y + t * d
+        };
+        //printf("arc center: %f %f\n", arc_center.x, arc_center.y);
+    }
+
     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 = atan2(-b, e) - current_angle;
     }
 
     angle_error = voss::norm_delta(angle_error);
@@ -67,11 +76,22 @@ ArcPIDController::get_command(bool reverse, bool thru,
     lin_speed *= reverse ? -1 : 1;
 
     double left_speed, right_speed;
-    if (t != 0.0) {
+    if (arc_radius != 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;
+        left_speed = lin_speed * (2 - track_width / arc_radius) / 2;
+        right_speed = lin_speed * (2 + track_width / arc_radius) / 2;
+        double tangent = atan2(current_pos.y - arc_center.y, current_pos.x - arc_center.x);
+        if ((bool)(arc_radius > 0) != reverse) {
+            tangent += M_PI_2;
+        } else {
+            tangent -= M_PI_2;
+        }
+        //printf("current %f target %f\n", voss::to_degrees(current_angle), voss::to_degrees(tangent));
+        printf("x %f y %f\n", current_pos.x, current_pos.y);
+        double ang_speed = angular_pid.update(voss::norm_delta(tangent - current_angle));
+        left_speed -= ang_speed;
+        right_speed += ang_speed;
     } else {
         left_speed = lin_speed;
         right_speed = lin_speed;
@@ -98,7 +118,9 @@ chassis::DiffChassisCommand ArcPIDController::get_angular_command(
 
 void ArcPIDController::reset() {
     this->linear_pid.reset();
+    this->angular_pid.reset();
     this->prev_lin_speed = 0.0;
+    this->arc_radius = NAN;
 }
 
 std::shared_ptr<ArcPIDController>

diff --git a/src/VOSS/controller/ArcPIDControllerBuilder.cpp b/src/VOSS/controller/ArcPIDControllerBuilder.cpp
@@ -31,6 +31,12 @@ ArcPIDControllerBuilder::with_linear_constants(double kP, double kI,
     return *this;
 }
 
+ArcPIDControllerBuilder&
+ArcPIDControllerBuilder::with_angular_constants(double kP, double kI, double kD) {
+    this->ctrl.angular_pid.set_constants(kP, kI, kD);
+    return *this;
+}
+
 ArcPIDControllerBuilder&
 ArcPIDControllerBuilder::with_track_width(double track_width) {
     this->ctrl.track_width = track_width;

diff --git a/src/VOSS/exit_conditions/SettleExitCondition.cpp b/src/VOSS/exit_conditions/SettleExitCondition.cpp
@@ -7,6 +7,7 @@
 
 namespace voss::controller {
 bool SettleExitCondition::is_met(Pose current_pose, bool thru) {
+    printf("initial %d current %d\n", initial_time, current_time);
     if (initial_time <= initial_delay) {
         initial_time += 10;
         return false;
@@ -19,14 +20,11 @@ bool SettleExitCondition::is_met(Pose current_pose, bool thru) {
                              this->prev_pose.theta.value()) <
                     voss::to_radians(this->tolerance)) {
                     this->current_time += 10;
+                } else {
+                    current_time = 0;
+                    prev_pose = current_pose;
                 }
             }
-
-            //            if(thru) {
-            //                printf("settle condition met\n");
-            //                return true;
-            //            }
-
         } else {
             current_time = 0;
             prev_pose = current_pose;

diff --git a/src/main.cpp b/src/main.cpp
@@ -42,9 +42,11 @@ auto swing = voss::controller::SwingControllerBuilder::new_builder(odom)
                  .build();
 
 auto arc = voss::controller::ArcPIDControllerBuilder(odom)
-               .with_track_width(14)
+               .with_track_width(16)
                .with_linear_constants(20, 0.02, 169)
+               .with_angular_constants(250, 0.05, 2435)
                .with_min_error(5)
+               .with_slew(8)
                .build();
 
 pros::Controller master(pros::E_CONTROLLER_MASTER);
@@ -141,26 +143,8 @@ void opcontrol() {
                        master.get_analog(ANALOG_RIGHT_X));
 
         if (master.get_digital_new_press(DIGITAL_Y)) {
-            odom->set_pose({0.0, 0.0, 270});
-            chassis.move({24, 24, 45}, boomerang, 100,
-                         voss::Flags::THRU | voss::Flags::REVERSE);
-            printf("1.\n");
-            master.rumble("--");
-            chassis.turn(90, 100, voss::Flags::THRU);
-            printf("2.\n");
-            master.rumble("--");
-            chassis.move({-10, 60, 180}, boomerang, 100, voss::Flags::THRU);
-            printf("3.\n");
-            master.rumble("--");
-            chassis.turn(270, swing, 100,
-                         voss::Flags::REVERSE | voss::Flags::THRU);
-            printf("4.\n");
-            master.rumble("--");
-            chassis.move({10, 30}, 100, voss::Flags::THRU);
-            printf("5.\n");
-            master.rumble("--");
-            chassis.turn(0);
-            printf("end\n");
+            odom->set_pose({0.0, 0.0, 90});
+            chassis.move({-24, 24}, arc);
         }
 
         pros::lcd::clear_line(1);