Skip to content

Commit 847075a

Browse files
AndrewLuGitgithub-actions[bot]
AndrewLuGit
and
github-actions[bot]
authored
Add PID util class (#82)
* create PID class * use PID class in controllers, fix issues with exit conditions * Committing clang-format changes --------- Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com>
1 parent b423a45 commit 847075a

20 files changed

+100
-142
lines changed

‎include/VOSS/controller/ArcPIDController.hpp

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,15 @@
11
#pragma once
22

33
#include "VOSS/controller/AbstractController.hpp"
4+
#include "VOSS/utils/PID.hpp"
45
#include <memory>
56

67
namespace voss::controller {
78

89
class ArcPIDController : public AbstractController {
910
protected:
1011
std::shared_ptr<ArcPIDController> p;
11-
double linear_kP, linear_kI, linear_kD;
12+
utils::PID linear_pid;
1213
double track_width;
1314
double min_error;
1415
double can_reverse;
@@ -23,8 +24,6 @@ class ArcPIDController : public AbstractController {
2324
public:
2425
ArcPIDController(std::shared_ptr<localizer::AbstractLocalizer> l);
2526

26-
double linear_pid(double error);
27-
2827
chassis::DiffChassisCommand
2928
get_command(bool reverse, bool thru,
3029
std::shared_ptr<AbstractExitCondition> ec) override;

‎include/VOSS/controller/BoomerangController.hpp

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include "VOSS/exit_conditions/AbstractExitCondition.hpp"
77
#include "VOSS/localizer/AbstractLocalizer.hpp"
88
#include "VOSS/utils/flags.hpp"
9+
#include "VOSS/utils/PID.hpp"
910

1011
namespace voss::controller {
1112

@@ -15,8 +16,7 @@ class BoomerangController : public AbstractController {
1516
double lead_pct;
1617
Pose carrotPoint;
1718

18-
double linear_kP, linear_kI, linear_kD;
19-
double angular_kP, angular_kI, angular_kD;
19+
utils::PID linear_pid, angular_pid;
2020
double vel;
2121
double exit_error;
2222
double angular_exit_error;
@@ -30,8 +30,6 @@ class BoomerangController : public AbstractController {
3030
double prev_angle;
3131
double min_vel;
3232

33-
double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err;
34-
3533
public:
3634
BoomerangController(std::shared_ptr<localizer::AbstractLocalizer> l);
3735

@@ -43,9 +41,6 @@ class BoomerangController : public AbstractController {
4341
voss::AngularDirection direction,
4442
std::shared_ptr<AbstractExitCondition> ec) override;
4543

46-
double linear_pid(double error);
47-
double angular_pid(double error);
48-
4944
void reset() override;
5045

5146
std::shared_ptr<BoomerangController>

‎include/VOSS/controller/PIDController.hpp

Lines changed: 2 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#pragma once
22

33
#include "VOSS/controller/AbstractController.hpp"
4+
#include "VOSS/utils/PID.hpp"
45
#include <memory>
56

67
namespace voss::controller {
@@ -9,23 +10,17 @@ class PIDController : public AbstractController {
910
protected:
1011
std::shared_ptr<PIDController> p;
1112

12-
double linear_kP, linear_kI, linear_kD;
13-
double angular_kP, angular_kI, angular_kD;
13+
utils::PID linear_pid, angular_pid;
1414
double tracking_kP;
1515
double min_error;
1616
bool can_reverse;
1717

1818
double min_vel;
1919
bool turn_overshoot;
2020

21-
double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err;
22-
2321
public:
2422
PIDController(std::shared_ptr<localizer::AbstractLocalizer> l);
2523

26-
double linear_pid(double error);
27-
double angular_pid(double error);
28-
2924
chassis::DiffChassisCommand
3025
get_command(bool reverse, bool thru,
3126
std::shared_ptr<AbstractExitCondition> ec) override;

‎include/VOSS/controller/SwingController.hpp

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,17 +2,17 @@
22

33
#include "PIDController.hpp"
44
#include "VOSS/controller/AbstractController.hpp"
5+
#include "VOSS/utils/PID.hpp"
56
#include <memory>
67

78
namespace voss::controller {
89
class SwingController : public AbstractController {
910
protected:
1011
std::shared_ptr<SwingController> p;
11-
double angular_kP, angular_kI, angular_kD;
12+
utils::PID angular_pid;
1213
bool can_reverse;
1314
bool turn_overshoot;
1415

15-
double prev_ang_err, total_ang_err;
1616
double prev_ang_speed;
1717

1818
public:
@@ -26,8 +26,6 @@ class SwingController : public AbstractController {
2626
voss::AngularDirection direction,
2727
std::shared_ptr<AbstractExitCondition> ec) override;
2828

29-
double angular_pid(double error);
30-
3129
void reset() override;
3230

3331
std::shared_ptr<SwingController>

‎include/VOSS/exit_conditions/SettleExitCondition.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ class SettleExitCondition : public AbstractExitCondition {
1616

1717
public:
1818
SettleExitCondition(int settle_time, double tolerance, int initial_delay);
19-
bool is_met(Pose current_pose, bool thru);
19+
bool is_met(Pose current_pose, bool thru) override;
2020
void reset() override;
2121
};
2222
} // namespace voss::controller

‎include/VOSS/exit_conditions/TimeOutExitCondition.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ class TimeOutExitCondition : public AbstractExitCondition {
1212

1313
public:
1414
TimeOutExitCondition(int timeout);
15-
bool is_met(Pose current_pose, bool thru);
15+
bool is_met(Pose current_pose, bool thru) override;
1616
void reset() override;
1717
};
1818
} // namespace voss::controller

‎include/VOSS/exit_conditions/ToleranceExitCondition.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ class ToleranceExitCondition : public AbstractExitCondition {
1111
std::shared_ptr<ToleranceLinearExitCondition> lin_exit = nullptr;
1212

1313
public:
14+
void set_target(voss::Pose target) override;
1415
bool is_met(Pose pose, bool thru) override;
1516
void add_ang_exit(double angular_tolerance);
1617
void add_lin_exit(double linear_tolerance);

‎include/VOSS/utils/PID.hpp

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#pragma once
2+
3+
namespace voss::utils {
4+
5+
class PID {
6+
private:
7+
double kP, kI, kD;
8+
double prev_error, total_error;
9+
10+
public:
11+
PID();
12+
PID(double kP, double kI, double kD);
13+
double update(double error);
14+
void reset();
15+
void set_constants(double kP, double kI, double kD);
16+
};
17+
18+
} // namespace voss::utils

‎src/VOSS/controller/ArcPIDController.cpp

Lines changed: 3 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ namespace voss::controller {
88

99
ArcPIDController::ArcPIDController(
1010
std::shared_ptr<localizer::AbstractLocalizer> l)
11-
: AbstractController(std::move(l)), prev_lin_err(0.0), total_lin_err(0.0) {
11+
: AbstractController(std::move(l)) {
1212
}
1313

1414
chassis::DiffChassisCommand
@@ -48,7 +48,7 @@ ArcPIDController::get_command(bool reverse, bool thru,
4848

4949
angle_error = voss::norm_delta(angle_error);
5050

51-
double lin_speed = thru ? 100.0 : this->linear_pid(distance_error);
51+
double lin_speed = thru ? 100.0 : this->linear_pid.update(distance_error);
5252

5353
if (distance_error < this->min_error) {
5454
this->can_reverse = true;
@@ -96,20 +96,8 @@ chassis::DiffChassisCommand ArcPIDController::get_angular_command(
9696
return chassis::DiffChassisCommand{chassis::Stop{}};
9797
}
9898

99-
double ArcPIDController::linear_pid(double error) {
100-
this->total_lin_err += error;
101-
102-
double speed = linear_kP * error + linear_kD * (error - prev_lin_err) +
103-
linear_kI * total_lin_err;
104-
105-
this->prev_lin_err = error;
106-
107-
return speed;
108-
}
109-
11099
void ArcPIDController::reset() {
111-
this->prev_lin_err = 0.0;
112-
this->total_lin_err = 0.0;
100+
this->linear_pid.reset();
113101
this->prev_lin_speed = 0.0;
114102
}
115103

‎src/VOSS/controller/ArcPIDControllerBuilder.cpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,7 @@ ArcPIDControllerBuilder ArcPIDControllerBuilder::from(ArcPIDController arc) {
2727
ArcPIDControllerBuilder&
2828
ArcPIDControllerBuilder::with_linear_constants(double kP, double kI,
2929
double kD) {
30-
this->ctrl.linear_kP = kP;
31-
this->ctrl.linear_kI = kI;
32-
this->ctrl.linear_kD = kD;
30+
this->ctrl.linear_pid.set_constants(kP, kI, kD);
3331
return *this;
3432
}
3533

‎src/VOSS/controller/BoomerangController.cpp

Lines changed: 5 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ BoomerangController::get_command(bool reverse, bool thru,
4444

4545
angle_error = voss::norm_delta(angle_error);
4646

47-
double lin_speed = linear_pid(distance_error);
47+
double lin_speed = linear_pid.update(distance_error);
4848
if (thru) {
4949
lin_speed = copysign(fmax(fabs(lin_speed), this->min_vel), lin_speed);
5050
}
@@ -63,7 +63,7 @@ BoomerangController::get_command(bool reverse, bool thru,
6363

6464
while (fabs(poseError) > M_PI)
6565
poseError -= 2 * M_PI * poseError / fabs(poseError);
66-
ang_speed = angular_pid(poseError);
66+
ang_speed = angular_pid.update(poseError);
6767
}
6868

6969
// reduce the linear speed if the bot is tangent to the target
@@ -75,7 +75,7 @@ BoomerangController::get_command(bool reverse, bool thru,
7575
lin_speed = -lin_speed;
7676
}
7777

78-
ang_speed = angular_pid(angle_error);
78+
ang_speed = angular_pid.update(angle_error);
7979
}
8080

8181
lin_speed *= cos(angle_error);
@@ -100,35 +100,9 @@ chassis::DiffChassisCommand BoomerangController::get_angular_command(
100100
return chassis::DiffChassisCommand{chassis::Stop{}};
101101
}
102102

103-
double BoomerangController::linear_pid(double error) {
104-
total_lin_err += error;
105-
106-
this->vel = error - prev_ang_err;
107-
108-
double speed = linear_kP * error + linear_kD * (error - prev_lin_err) +
109-
linear_kI * total_lin_err;
110-
111-
this->prev_lin_err = error;
112-
113-
return speed;
114-
}
115-
116-
double BoomerangController::angular_pid(double error) {
117-
total_ang_err += error;
118-
119-
double speed = angular_kP * error + angular_kD * (error - prev_ang_err) +
120-
angular_kI * total_ang_err;
121-
122-
this->prev_ang_err = error;
123-
124-
return speed;
125-
}
126-
127103
void BoomerangController::reset() {
128-
this->prev_lin_err = 0;
129-
this->total_lin_err = 0;
130-
this->prev_ang_err = 0;
131-
this->total_ang_err = 0;
104+
this->linear_pid.reset();
105+
this->angular_pid.reset();
132106
this->can_reverse = false;
133107
this->counter = 0;
134108
}

‎src/VOSS/controller/BoomerangControllerBuilder.cpp

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -29,18 +29,14 @@ BoomerangControllerBuilder::from(BoomerangController bmr) {
2929
BoomerangControllerBuilder&
3030
BoomerangControllerBuilder::with_linear_constants(double kP, double kI,
3131
double kD) {
32-
this->ctrl.linear_kP = kP;
33-
this->ctrl.linear_kI = kI;
34-
this->ctrl.linear_kD = kD;
32+
this->ctrl.linear_pid.set_constants(kP, kI, kD);
3533
return *this;
3634
}
3735

3836
BoomerangControllerBuilder&
3937
BoomerangControllerBuilder::with_angular_constants(double kP, double kI,
4038
double kD) {
41-
this->ctrl.angular_kP = kP;
42-
this->ctrl.angular_kI = kI;
43-
this->ctrl.angular_kD = kD;
39+
this->ctrl.angular_pid.set_constants(kP, kI, kD);
4440
return *this;
4541
}
4642

‎src/VOSS/controller/PIDController.cpp

Lines changed: 8 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,7 @@
99
namespace voss::controller {
1010

1111
PIDController::PIDController(std::shared_ptr<localizer::AbstractLocalizer> l)
12-
: AbstractController(std::move(l)), prev_lin_err(0.0), total_lin_err(0.0),
13-
prev_ang_err(0.0), total_ang_err(0.0) {
12+
: AbstractController(std::move(l)) {
1413
}
1514

1615
chassis::DiffChassisCommand
@@ -43,7 +42,8 @@ PIDController::get_command(bool reverse, bool thru,
4342

4443
angle_error = voss::norm_delta(angle_error);
4544

46-
double lin_speed = (thru ? 100.0 : (linear_pid(distance_error))) * dir;
45+
double lin_speed =
46+
(thru ? 100.0 : (linear_pid.update(distance_error))) * dir;
4747

4848
double ang_speed;
4949
if (distance_error < min_error) {
@@ -58,7 +58,7 @@ PIDController::get_command(bool reverse, bool thru,
5858

5959
while (fabs(poseError) > M_PI)
6060
poseError -= 2 * M_PI * poseError / fabs(poseError);
61-
ang_speed = angular_pid(poseError);
61+
ang_speed = angular_pid.update(poseError);
6262
}
6363

6464
// reduce the linear speed if the bot is tangent to the target
@@ -71,7 +71,7 @@ PIDController::get_command(bool reverse, bool thru,
7171
lin_speed = -lin_speed;
7272
}
7373

74-
ang_speed = angular_pid(angle_error);
74+
ang_speed = angular_pid.update(angle_error);
7575
}
7676
// Runs at the end of a through movement
7777
if (ec->is_met(this->l->get_pose(), thru)) {
@@ -129,7 +129,7 @@ PIDController::get_angular_command(bool reverse, bool thru,
129129
}
130130
}
131131

132-
double ang_speed = angular_pid(angular_error);
132+
double ang_speed = angular_pid.update(angular_error);
133133
if (ec->is_met(this->l->get_pose(), thru) || chainedExecutable) {
134134
if (thru) {
135135
return chassis::DiffChassisCommand{
@@ -140,37 +140,11 @@ PIDController::get_angular_command(bool reverse, bool thru,
140140
return chassis::DiffChassisCommand{
141141
chassis::diff_commands::Voltages{-ang_speed, ang_speed}};
142142
}
143-
// What is calculating the required motor power for a linear movement
144-
// Returns value for motor power with type double
145-
double PIDController::linear_pid(double error) {
146-
total_lin_err += error;
147-
148-
double speed = linear_kP * error + linear_kD * (error - prev_lin_err) +
149-
linear_kI * total_lin_err;
150-
151-
this->prev_lin_err = error;
152-
153-
return speed;
154-
}
155-
// What is calculating the required motor power for a turn
156-
// Returns value for motor power with type double
157-
double PIDController::angular_pid(double error) {
158-
total_ang_err += error;
159-
160-
double speed = angular_kP * error + angular_kD * (error - prev_ang_err) +
161-
angular_kI * total_ang_err;
162-
163-
this->prev_ang_err = error;
164-
165-
return speed;
166-
}
167143

168144
// Function to reset every variable used in the PID controller
169145
void PIDController::reset() {
170-
this->prev_lin_err = 0;
171-
this->total_lin_err = 0;
172-
this->prev_ang_err = 0;
173-
this->total_ang_err = 0;
146+
this->linear_pid.reset();
147+
this->angular_pid.reset();
174148
this->can_reverse = false;
175149
this->turn_overshoot = false;
176150
}

0 commit comments

Comments
 (0)