Skip to content

Commit

Permalink
Improve PID class (#128)
Browse files Browse the repository at this point in the history 
Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com>
  • Loading branch information
@jballoffet
jballoffet committed Jul 28, 2023
1 parent 6babee5 commit d73e2eb
Show file tree
Hide file tree
Showing 3 changed files with 119 additions and 113 deletions.
26 changes: 12 additions & 14 deletions andino_firmware/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,8 +124,8 @@ andino::Motor right_motor(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_
RIGHT_MOTOR_BACKWARD_GPIO_PIN);

// TODO(jballoffet): Make these objects local to the main function.
andino::PID left_pid_controller;
andino::PID right_pid_controller;
andino::PID left_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM);
andino::PID right_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM);

/* Clear the current command parameters */
void resetCommand() {
Expand Down Expand Up @@ -194,11 +194,11 @@ int runCommand() {
right_motor.set_speed(0);
left_pid_controller.reset(readEncoder(LEFT));
right_pid_controller.reset(readEncoder(RIGHT));
left_pid_controller.set_state(false);
right_pid_controller.set_state(false);
left_pid_controller.enable(false);
right_pid_controller.enable(false);
} else {
left_pid_controller.set_state(true);
right_pid_controller.set_state(true);
left_pid_controller.enable(true);
right_pid_controller.enable(true);
}
// The target speeds are in ticks per second, so we need to convert them
// to ticks per PID_INTERVAL
Expand All @@ -212,8 +212,8 @@ int runCommand() {
left_pid_controller.reset(readEncoder(LEFT));
right_pid_controller.reset(readEncoder(RIGHT));
// Sneaky way to temporarily disable the PID
left_pid_controller.set_state(false);
right_pid_controller.set_state(false);
left_pid_controller.enable(false);
right_pid_controller.enable(false);
left_motor.set_speed(arg1);
right_motor.set_speed(arg2);
Serial.println("OK");
Expand Down Expand Up @@ -254,8 +254,6 @@ void setup() {

left_pid_controller.reset(readEncoder(LEFT));
right_pid_controller.reset(readEncoder(RIGHT));
left_pid_controller.set_output_limits(-MAX_PWM, MAX_PWM);
right_pid_controller.set_output_limits(-MAX_PWM, MAX_PWM);
}

/* Enter the main loop. Read and parse input from the serial port
Expand Down Expand Up @@ -305,8 +303,8 @@ void loop() {
// Run a PID calculation at the appropriate intervals
if (millis() > nextPID) {
int left_motor_speed, right_motor_speed;
left_pid_controller.update(readEncoder(LEFT), left_motor_speed);
right_pid_controller.update(readEncoder(RIGHT), right_motor_speed);
left_pid_controller.compute(readEncoder(LEFT), left_motor_speed);
right_pid_controller.compute(readEncoder(RIGHT), right_motor_speed);
left_motor.set_speed(left_motor_speed);
right_motor.set_speed(right_motor_speed);
nextPID += PID_INTERVAL;
Expand All @@ -316,7 +314,7 @@ void loop() {
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
left_motor.set_speed(0);
right_motor.set_speed(0);
left_pid_controller.set_state(false);
right_pid_controller.set_state(false);
left_pid_controller.enable(false);
right_pid_controller.enable(false);
}
}
118 changes: 39 additions & 79 deletions andino_firmware/src/pid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,95 +64,59 @@
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "pid.h"

// TODO(jballoffet): Revisit overall logic and add proper documentation.
namespace andino {

/*
* Initialize PID variables to zero to prevent startup spikes
* when turning PID on to start moving
* In particular, assign both encoder_ and prev_enc_ the current encoder value
* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* Note that the assumption here is that PID is only turned on
* when going from stop to moving, that's why we can init everything on zero.
*/
// Note: see the following links for more information regarding this class implementation:
// - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
// - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
// - http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/

void PID::reset(int encoder_count) {
target_ticks_per_frame_ = 0.0;
encoder_ = encoder_count;
prev_enc_ = encoder_;
output_ = 0;
prev_input_ = 0;
i_term_ = 0;
// Since we can assume that the PID is only turned on when going from stop to moving, we can init
// everything on zero.
setpoint_ = 0;
integral_term_ = 0;
last_encoder_count_ = encoder_count;
last_input_ = 0;
last_output_ = 0;
}

/* PID routine to compute the next motor commands */
void PID::compute() {
long Perror;
long output;
int input;

// Perror = target_ticks_per_frame_ - (encoder_ - prev_enc_);
input = encoder_ - prev_enc_;
Perror = target_ticks_per_frame_ - input;

/*
* Avoid derivative kick and allow tuning changes,
* see
* http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see
* http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
// output = (Kp * Perror + Kd * (Perror - PrevErr) + Ki * Ierror) / Ko;
// PrevErr = Perror;
output = (kp_ * Perror - kd_ * (input - prev_input_) + i_term_) / ko_;
prev_enc_ = encoder_;
void PID::enable(bool enabled) { enabled_ = enabled; }

output += output_;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= output_limit_max_)
output = output_limit_max_;
else if (output <= output_limit_min_)
output = output_limit_min_;
else
/*
* allow turning changes, see
* http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
i_term_ += ki_ * Perror;
void PID::compute(int encoder_count, int& computed_output) {
if (!enabled_) {
// Reset PID once to prevent startup spikes.
if (last_input_ != 0) {
reset(encoder_count);
}
return;
}

output_ = output;
prev_input_ = input;
}
int input = encoder_count - last_encoder_count_;
long error = setpoint_ - input;

/* Read the encoder values and call the PID routine */
void PID::update(int encoder_count, int& motor_speed) {
/* Read the encoders */
encoder_ = encoder_count;
long output = (kp_ * error - kd_ * (input - last_input_) + integral_term_) / ko_;
output += last_output_;

/* If we're not moving there is nothing more to do */
if (!enabled_) {
/*
* Reset PIDs once, to prevent startup spikes,
* see
* http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* prev_input_ is considered a good proxy to detect
* whether reset has already happened
*/
if (prev_input_ != 0) reset(encoder_count);
return;
// Accumulate integral term as long as output doesn't saturate.
if (output >= output_max_) {
output = output_max_;
} else if (output <= output_min_) {
output = output_min_;
} else {
integral_term_ += ki_ * error;
}

/* Compute PID update for each motor */
compute();
// Set the computed output accordingly.
computed_output = output;

/* Set the motor speeds accordingly */
motor_speed = output_;
// Store obtained values.
last_encoder_count_ = encoder_count;
last_input_ = input;
last_output_ = output;
}

void PID::set_output_limits(int min, int max) {
output_limit_min_ = min;
output_limit_max_ = max;
}
void PID::set_setpoint(int setpoint) { setpoint_ = setpoint; }

void PID::set_tunings(int kp, int kd, int ki, int ko) {
kp_ = kp;
Expand All @@ -161,8 +125,4 @@ void PID::set_tunings(int kp, int kd, int ki, int ko) {
ko_ = ko;
}

void PID::set_state(bool enabled) { enabled_ = enabled; }

void PID::set_setpoint(int setpoint) { target_ticks_per_frame_ = setpoint; }

} // namespace andino
88 changes: 68 additions & 20 deletions andino_firmware/src/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,35 +31,83 @@

namespace andino {

// TODO(jballoffet): Revisit overall logic and add proper documentation.
/// @brief This class provides a simple PID controller implementation.
class PID {
public:
/// @brief Constructs a new PID object.
///
/// @param kp Tuning proportional gain.
/// @param kd Tuning derivative gain.
/// @param ki Tuning integral gain.
/// @param ko Tuning output gain.
/// @param output_min Output minimum limit.
/// @param output_max Output maximum limit.
PID(int kp, int kd, int ki, int ko, int output_min, int output_max)
: kp_(kp),
kd_(kd),
ki_(ki),
ko_(ko),
output_min_(output_min),
output_max_(output_max),
enabled_(false) {}

/// @brief Resets the PID controller.
///
/// @param encoder_count Current encoder value.
void reset(int encoder_count);
void compute();
void update(int encoder_count, int& motor_speed);
void set_output_limits(int min, int max);
void set_tunings(int kp, int kd, int ki, int ko);
void set_state(bool enabled);

/// @brief Enables the PID controller.
///
/// @param enabled True to enable the PID, false otherwise.
void enable(bool enabled);

/// @brief Computes a new output.
///
/// @param encoder_count Current encoder value.
/// @param computed_output Computed output value.
void compute(int encoder_count, int& computed_output);

/// @brief Sets the setpoint.
///
/// @param setpoint Desired setpoint value.
void set_setpoint(int setpoint);

/// @brief Sets the tuning gains.
///
/// @param kp Tuning proportional gain.
/// @param kd Tuning derivative gain.
/// @param ki Tuning integral gain.
/// @param ko Tuning output gain.
void set_tunings(int kp, int kd, int ki, int ko);

private:
int target_ticks_per_frame_; // target speed in ticks per frame
long encoder_; // encoder count
long prev_enc_; // last encoder count
int prev_input_; // last input
int i_term_; // integrated term
long output_; // last motor setting
/// Tuning proportional gain.
int kp_;
/// Tuning derivative gain.
int kd_;
/// Tuning integral gain.
int ki_;
/// Tuning output gain.
int ko_;

/* PID Parameters */
int kp_ = 30;
int kd_ = 10;
int ki_ = 0;
int ko_ = 10;
/// Output minimum limit.
int output_min_;
/// Output maximum limit.
int output_max_;

bool enabled_ = false; // is the base in motion?
/// True if the PID is enabled, false otherwise.
bool enabled_;

int output_limit_min_;
int output_limit_max_;
/// Setpoint value.
int setpoint_;
/// Accumulated integral term.
int integral_term_;
/// Last received encoder value.
long last_encoder_count_;
/// Last computed input value.
int last_input_;
/// Last computed output value.
long last_output_;
};

} // namespace andino

0 comments on commit d73e2eb

Please sign in to comment.