diff --git a/include/VOSS/controller/ArcPIDController.hpp b/include/VOSS/controller/ArcPIDController.hpp index 724ba2f7..f6ba32fd 100644 --- a/include/VOSS/controller/ArcPIDController.hpp +++ b/include/VOSS/controller/ArcPIDController.hpp @@ -10,9 +10,12 @@ class ArcPIDController : public AbstractController { protected: std::shared_ptr 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 index cdfb9a0d..9edbdecf 100644 --- 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 index 44b5e6c9..a53bb5ab 100644 --- 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 diff --git a/src/VOSS/controller/ArcPIDControllerBuilder.cpp b/src/VOSS/controller/ArcPIDControllerBuilder.cpp index 4a87cc6e..5749c80d 100644 --- 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 index ba531181..1150ae6c 100644 --- 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 index fa28423f..9345ce25 100644 --- 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);