Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add angular PID to ArcPIDController, fix issue with SettleExitCondition #89

Merged
merged 1 commit into from
Mar 23, 2024
Merged

Add angular PID to ArcPIDController, fix issue with SettleExitCondition #89

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions include/VOSS/controller/ArcPIDController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,12 @@ class ArcPIDController : public AbstractController {
protected:
std::shared_ptr<ArcPIDController> p;
utils::PID linear_pid;
utils::PID angular_pid;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: member variable 'angular_pid' has protected visibility [cppcoreguidelines-non-private-member-variables-in-classes]

    utils::PID angular_pid;
               ^

double track_width;
double min_error;
double can_reverse;
double arc_radius;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: member variable 'arc_radius' has protected visibility [cppcoreguidelines-non-private-member-variables-in-classes]

    double arc_radius;
           ^

Point arc_center;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: member variable 'arc_center' has protected visibility [cppcoreguidelines-non-private-member-variables-in-classes]

    Point arc_center;
          ^

double prev_t;
double slew;
double prev_lin_speed;
Expand Down
1 change: 1 addition & 0 deletions include/VOSS/controller/ArcPIDControllerBuilder.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: parameter name 'kP' is too short, expected at least 3 characters [readability-identifier-length]

    ArcPIDControllerBuilder& with_angular_constants(double kP, double kI, double kD);
                                                           ^

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: parameter name 'kI' is too short, expected at least 3 characters [readability-identifier-length]

    ArcPIDControllerBuilder& with_angular_constants(double kP, double kI, double kD);
                                                                      ^

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: parameter name 'kD' is too short, expected at least 3 characters [readability-identifier-length]

    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
Expand Down
30 changes: 26 additions & 4 deletions src/VOSS/controller/ArcPIDController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: if with identical then and else branches [bugprone-branch-clone]

        if ((bool)(arc_radius > 0) != reverse) {
        ^
Additional context

src/VOSS/controller/ArcPIDController.cpp:86: else branch starts here

        } else {
          ^

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));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: variable 'ang_speed' of type 'double' can be declared 'const' [misc-const-correctness]

Suggested change
double ang_speed = angular_pid.update(voss::norm_delta(tangent - current_angle));
double const 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;
Expand All @@ -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>
Expand Down
6 changes: 6 additions & 0 deletions src/VOSS/controller/ArcPIDControllerBuilder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
10 changes: 4 additions & 6 deletions src/VOSS/exit_conditions/SettleExitCondition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
26 changes: 5 additions & 21 deletions src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 16 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

               .with_track_width(16)
                                 ^

.with_linear_constants(20, 0.02, 169)
.with_angular_constants(250, 0.05, 2435)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 250 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

               .with_angular_constants(250, 0.05, 2435)
                                       ^

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 0.05 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

               .with_angular_constants(250, 0.05, 2435)
                                            ^

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 2435 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

               .with_angular_constants(250, 0.05, 2435)
                                                  ^

.with_min_error(5)
.with_slew(8)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 8 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

               .with_slew(8)
                          ^

.build();

pros::Controller master(pros::E_CONTROLLER_MASTER);
Expand Down Expand Up @@ -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});
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 90 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

            odom->set_pose({0.0, 0.0, 90});
                                      ^

chassis.move({-24, 24}, arc);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 24 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

            chassis.move({-24, 24}, arc);
                           ^

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

warning: 24 is a magic number; consider replacing it with a named constant [cppcoreguidelines-avoid-magic-numbers]

            chassis.move({-24, 24}, arc);
                               ^

}

pros::lcd::clear_line(1);
Expand Down