Skip to content

Commit

Permalink
Only send motor commands if pid is enabled
Browse files Browse the repository at this point in the history
Signed-off-by: Gary Servin <[email protected]>
  • Loading branch information
garyservin committed Feb 12, 2024
1 parent 91e6029 commit 6ac2f43
Show file tree
Hide file tree
Showing 3 changed files with 29 additions and 14 deletions.
24 changes: 14 additions & 10 deletions andino_firmware/src/app.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -168,15 +168,19 @@ void App::adjust_motors_speed() {
int right_motor_speed = 0;
left_pid_controller_.compute(left_encoder_.read(), left_motor_speed);
right_pid_controller_.compute(right_encoder_.read(), right_motor_speed);
left_motor_.set_speed(left_motor_speed);
right_motor_.set_speed(right_motor_speed);
if (left_pid_controller_.enabled()){
left_motor_.set_speed(left_motor_speed);
}
if (right_pid_controller_.enabled()){
right_motor_.set_speed(right_motor_speed);
}
}

void App::stop_motors() {
left_motor_.set_speed(0);
right_motor_.set_speed(0);
left_pid_controller_.enable(false);
right_pid_controller_.enable(false);
left_pid_controller_.disable();
right_pid_controller_.disable();
}

void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); }
Expand Down Expand Up @@ -228,11 +232,11 @@ void App::cmd_set_motors_speed_cb(int argc, char** argv) {
right_motor_.set_speed(0);
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
left_pid_controller_.enable(false);
right_pid_controller_.enable(false);
left_pid_controller_.disable();
right_pid_controller_.disable();
} else {
left_pid_controller_.enable(true);
right_pid_controller_.enable(true);
left_pid_controller_.enable();
right_pid_controller_.enable();
}

// The target speeds are in ticks per second, so we need to convert them to ticks per
Expand All @@ -253,8 +257,8 @@ void App::cmd_set_motors_pwm_cb(int argc, char** argv) {
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
// Sneaky way to temporarily disable the PID.
left_pid_controller_.enable(false);
right_pid_controller_.enable(false);
left_pid_controller_.disable();
right_pid_controller_.disable();
left_motor_.set_speed(left_motor_pwm);
right_motor_.set_speed(right_motor_pwm);
Serial.println("OK");
Expand Down
9 changes: 8 additions & 1 deletion andino_firmware/src/pid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,14 @@ void PID::reset(int encoder_count) {
last_output_ = 0;
}

void PID::enable(bool enabled) { enabled_ = enabled; }
/// @brief Enable PID
void PID::enable() { enabled_ = true; }

/// @brief Is the PID controller enabled?
bool PID::enabled() { return enabled_; }

/// @brief Disable PID
void PID::disable() { enabled_ = false; }

void PID::compute(int encoder_count, int& computed_output) {
if (!enabled_) {
Expand Down
10 changes: 7 additions & 3 deletions andino_firmware/src/pid.h
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,14 @@ class PID {
/// @param encoder_count Current encoder value.
void reset(int encoder_count);

/// @brief Returns if the PID controller is enabled or not.
bool enabled();

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

/// @brief Disables the PID controller.
void disable();

/// @brief Computes a new output.
///
Expand Down

0 comments on commit 6ac2f43

Please sign in to comment.