Skip to content

Commit

Permalink
[BLDC] Split up FOC Controller
Browse files Browse the repository at this point in the history 
- The computations related to the FOC control scheme are now isolated and distinct from the BLDC control loop.
- Moved things into their own folders
  • Loading branch information
@sahil-kale
sahil-kale committed Jan 6, 2024
1 parent 75b4e22 commit afb9df5
Show file tree
Hide file tree
Showing 17 changed files with 289 additions and 185 deletions.
4 changes: 3 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,9 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/hal)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/hwbridge/3phase)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/hwbridge/h_bridge)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/bldc)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/bldc/foc)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/bldc/6step)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/bldc/rotor_estimator)
# include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/brushed)
# include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/stepper)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util)
Expand Down
0 control_loop/bldc/6step_util.cpp → control_loop/bldc/6step/6step_util.cpp
View file
File renamed without changes.
0 control_loop/bldc/6step_util.hpp → control_loop/bldc/6step/6step_util.hpp
View file
File renamed without changes.
0 ...oop/bldc/brushless_6step_control_loop.cpp → ...dc/6step/brushless_6step_control_loop.cpp
View file
File renamed without changes.
0 ...oop/bldc/brushless_6step_control_loop.hpp → ...dc/6step/brushless_6step_control_loop.hpp
View file
File renamed without changes.
97 changes: 19 additions & 78 deletions ..._loop/bldc/brushless_foc_control_loop.cpp → ...p/bldc/foc/brushless_foc_control_loop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include "util.hpp"

namespace control_loop {
using namespace BldcFoc;

// Define the init function
void BrushlessFOCControlLoop::init(BrushlessFOCControlLoop::BrushlessFOCControlLoopParams* params) {
Expand All @@ -18,10 +19,6 @@ void BrushlessFOCControlLoop::init(BrushlessFOCControlLoop::BrushlessFOCControlL
break; // Don't initialize if there was an issue resetting the rotor position estimators
}

// Reset the PID controllers
pid_d_current_.reset();
pid_q_current_.reset();

// reset the status
status_.reset();

Expand Down Expand Up @@ -81,7 +78,7 @@ ControlLoop::ControlLoopBaseStatus BrushlessFOCControlLoop::run_current_control(
i_direct_quad_ref.quadrature = i_q_reference;

// Update the FOC inputs
BrushlessFOCControlLoop::FOCInputs foc_inputs =
FOCController::FOCInputs foc_inputs =
update_foc_inputs(current_time_us, last_run_time_, rotor_position_estimators_, num_rotor_position_estimators_,
bridge_, status_, foc_frame_vars_.duty_cycle_result.V_alpha_beta, params_, i_direct_quad_ref);

Expand All @@ -106,7 +103,7 @@ ControlLoop::ControlLoopBaseStatus BrushlessFOCControlLoop::run_current_control(
if (status_ == ControlLoop::ControlLoopBaseStatus::ERROR) {
break;
}
run_foc(foc_inputs, phase_commands);
foc_frame_vars_ = foc_controller_.run_foc(foc_inputs, phase_commands);
} break;

default:
Expand Down Expand Up @@ -197,95 +194,32 @@ void BrushlessFOCControlLoop::enter_state(const BrushlessFOCControlLoopState& cu
IGNORE(current_state);
switch (desired_state) {
case BrushlessFOCControlLoop::BrushlessFOCControlLoopState::RUN: {
// reset the PID controllers
pid_d_current_.reset();
pid_q_current_.reset();

// Set the PI gains
const float kp = params_->foc_params.current_control_bandwidth_rad_per_sec * params_->foc_params.phase_inductance;
const float ki = params_->foc_params.phase_resistance / params_->foc_params.phase_inductance *
kp; // multiplied by kp to create a series PI controller
float ki = params_->foc_params.phase_resistance / params_->foc_params.phase_inductance *
kp; // multiplied by kp to create a series PI controller

pid_d_current_.set_kp(kp);
pid_q_current_.set_kp(kp);
if (params_->foc_params.disable_ki == false) {
pid_d_current_.set_ki(ki);
pid_q_current_.set_ki(ki);
} else {
pid_d_current_.set_ki(0.0f);
pid_q_current_.set_ki(0.0f);
ki = 0.0f;
}

foc_controller_.init(kp, ki);

// reset the rotor position estimator
const bool position_estimators_reset_succesfully =
reset_position_estimators(rotor_position_estimators_, num_rotor_position_estimators_);
// TODO: Add a warning if the position estimators failed to reset
IGNORE(position_estimators_reset_succesfully);

// Reset the internal state
foc_frame_vars_ = FOCFrameVars();
foc_frame_vars_ = FOCController::FOCFrameVars();
} break;
case BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP:
default:
break;
}
}

void BrushlessFOCControlLoop::run_foc(BrushlessFOCControlLoop::FOCInputs foc_inputs,
hwbridge::Bridge3Phase::phase_command_t phase_commands[3]) {
do {
foc_frame_vars_.control_loop_type = get_desired_control_loop_type(foc_inputs.rotor_position_valid);
switch (foc_frame_vars_.control_loop_type) {
case BrushlessFOCControlLoopType::OPEN_LOOP: {
// Advance the angle
foc_frame_vars_.commanded_rotor_theta = BldcFoc::advance_open_loop_angle(
foc_frame_vars_.commanded_rotor_theta, params_->open_loop_full_speed_theta_velocity, foc_inputs.dt);
// Make the Vq equal to the param'ed value for drive voltage
foc_frame_vars_.V_direct_quad.quadrature = params_->open_loop_quadrature_voltage;
// Make the Vd equal to 0
foc_frame_vars_.V_direct_quad.direct = 0.0f;
} break;
case BrushlessFOCControlLoopType::CLOSED_LOOP: {
// Run the PI controller
const float q_voltage_delta =
pid_q_current_.calculate(foc_inputs.i_direct_quad.quadrature, foc_inputs.i_direct_quad_ref.quadrature);
const float d_voltage_delta =
pid_d_current_.calculate(foc_inputs.i_direct_quad.direct, foc_inputs.i_direct_quad_ref.direct);
foc_frame_vars_.V_direct_quad.quadrature += q_voltage_delta;
foc_frame_vars_.V_direct_quad.direct += d_voltage_delta;

// Clamp the Vq and Vd
foc_frame_vars_.V_direct_quad = BldcFoc::clamp_Vdq(foc_frame_vars_.V_direct_quad, foc_inputs.bus_voltage);

// Keep this around for the open loop case
foc_frame_vars_.commanded_rotor_theta = foc_inputs.theta_e;
} break;
default:
break;
}

// Determine the appropriate duty cycles for the inverter
BldcFoc::FocDutyCycleResult result = BldcFoc::determine_inverter_duty_cycles_foc(
foc_frame_vars_.commanded_rotor_theta, foc_frame_vars_.V_direct_quad, foc_inputs.bus_voltage,
params_->foc_params.pwm_control_type, phase_commands);

// Set the debug vars
foc_frame_vars_.foc_inputs = foc_inputs;
foc_frame_vars_.duty_cycle_result = result;

} while (false);
}

BrushlessFOCControlLoop::BrushlessFOCControlLoopType BrushlessFOCControlLoop::get_desired_control_loop_type(
bool is_any_estimator_valid) {
BrushlessFOCControlLoop::BrushlessFOCControlLoopType desired_control_loop_type =
BrushlessFOCControlLoop::BrushlessFOCControlLoopType::OPEN_LOOP;
if (is_any_estimator_valid) {
desired_control_loop_type = BrushlessFOCControlLoop::BrushlessFOCControlLoopType::CLOSED_LOOP;
}
return desired_control_loop_type;
}

bool BrushlessFOCControlLoop::reset_position_estimators(
bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[], size_t num_rotor_position_estimators) {
bool ret = false;
Expand Down Expand Up @@ -317,12 +251,12 @@ bool BrushlessFOCControlLoop::reset_position_estimators(
return ret;
}

BrushlessFOCControlLoop::FOCInputs BrushlessFOCControlLoop::update_foc_inputs(
FOCController::FOCInputs BrushlessFOCControlLoop::update_foc_inputs(
utime_t current_time_us, utime_t last_run_time_us,
bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[], size_t num_rotor_position_estimators,
hwbridge::Bridge3Phase& bridge, BrushlessFOCControlLoopStatus& status, math::alpha_beta_t V_alpha_beta,
const BrushlessFOCControlLoopParams* params, math::direct_quad_t i_direct_quad_ref) {
BrushlessFOCControlLoop::FOCInputs foc_inputs;
FOCController::FOCInputs foc_inputs;
do {
foc_inputs.timestamp = current_time_us;
foc_inputs.dt = (current_time_us - last_run_time_us) / 1e6f;
Expand Down Expand Up @@ -360,11 +294,18 @@ BrushlessFOCControlLoop::FOCInputs BrushlessFOCControlLoop::update_foc_inputs(

foc_inputs.i_direct_quad_ref = i_direct_quad_ref;

// Set the open loop params
foc_inputs.open_loop_theta_velocity = params->open_loop_theta_velocity;
foc_inputs.open_loop_quadrature_voltage = params->open_loop_quadrature_voltage;

// Set the PWM control type
foc_inputs.pwm_control_type = params->foc_params.pwm_control_type;

} while (false);

return foc_inputs;
}

BrushlessFOCControlLoop::FOCFrameVars BrushlessFOCControlLoop::get_foc_frame_computation() const { return foc_frame_vars_; }
FOCController::FOCFrameVars BrushlessFOCControlLoop::get_foc_frame_computation() const { return foc_frame_vars_; }

} // namespace control_loop
116 changes: 19 additions & 97 deletions ..._loop/bldc/brushless_foc_control_loop.hpp → ...p/bldc/foc/brushless_foc_control_loop.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

#include "bridge_3phase.hpp"
#include "control_loop.hpp"
#include "foc_controller.hpp"
#include "foc_util.hpp"
#include "hal_clock.hpp"
#include "math_foc.hpp"
Expand All @@ -15,6 +16,8 @@

namespace control_loop {

using namespace BldcFoc;

/**
* @brief A control loop for a brushless motor
*/
Expand All @@ -30,16 +33,6 @@ class BrushlessFOCControlLoop : public ControlLoop {
RUN,
};

/**
* @brief The type of control loop control
*/
enum class BrushlessFOCControlLoopType {
/// The control loop is open loop (not relying on rotor position estimation)
OPEN_LOOP,
/// The current control loop is in closed loop control
CLOSED_LOOP,
};

/**
* @brief The parameters used by the FOC control loop
*/
Expand Down Expand Up @@ -84,15 +77,10 @@ class BrushlessFOCControlLoop : public ControlLoop {
*/
float i_d_reference_default;

/**
* @brief The cutoff frequency of the low pass filter for the current controller (Hz)
*/
float current_lpf_fc;

/**
* @brief The PWM control type to use for the FOC control loop
*/
BldcFoc::BrushlessFocPwmControlType pwm_control_type;
BrushlessFocPwmControlType pwm_control_type;
};

/**
Expand All @@ -105,7 +93,7 @@ class BrushlessFOCControlLoop : public ControlLoop {
*/
BrushlessFocControLoopParams foc_params;
/// @brief The open loop full speed theta velocity (rad/s)
float open_loop_full_speed_theta_velocity;
float open_loop_theta_velocity;
/// @brief The magnitude of the direct voltage vector to apply in open loop mode
float open_loop_quadrature_voltage;
};
Expand Down Expand Up @@ -161,8 +149,7 @@ class BrushlessFOCControlLoop : public ControlLoop {
clock_(clock),
rotor_position_estimators_(rotor_position_estimators),
num_rotor_position_estimators_(num_rotor_position_estimators),
pid_q_current_(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, clock),
pid_d_current_(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, clock),
foc_controller_(clock),
foc_frame_vars_() {}

/**
Expand Down Expand Up @@ -195,62 +182,11 @@ class BrushlessFOCControlLoop : public ControlLoop {
*/
ControlLoop::ControlLoopBaseStatus run_current_control(float i_d_reference, float i_q_reference);

/**
* @brief Inputs to the FOC control loop
*/
class FOCInputs {
public:
/// @brief timestamp of the FOC calculation
utime_t timestamp = 0;
/// @brief The time since the last FOC calculation (seconds)
float dt = 0.0f;
/// @brief The rotor position at the time of the FOC calculation (radians)
float theta_e = 0.0f;
/// @brief Whether the rotor position is valid at the time of the FOC calculation
bool rotor_position_valid = false;

/// @brief The DC bus voltage at the time of the FOC calculation (Volts)
float bus_voltage = 0.0f;
/// @brief Whether the DC bus voltage is valid at the time of the FOC calculation
bool bus_voltage_valid = false;

/// @brief Whether the current measurements and derived calculations are valid at the time of the FOC calculation
bool current_measurements_valid = false;
/// @brief The alpha/beta current vector at the time of the FOC calculation inferred from the phase currents (A)
math::alpha_beta_t i_alpha_beta;
/// @brief The the direct/quadrature current at the time of the FOC calculation (A)
math::direct_quad_t i_direct_quad;

/// @brief The desired direct/quadrature current reference at the time of the FOC calculation (A)
math::direct_quad_t i_direct_quad_ref;
};

/**
* @brief FOC Computation 'Frame' that contains the inputs and outputs of the FOC computation
*/
class FOCFrameVars {
public:
/// @brief The inputs to the FOC computation
FOCInputs foc_inputs;

// Outputs
/// @brief The control loop type that was used in this FOC computation
BrushlessFOCControlLoopType control_loop_type = BrushlessFOCControlLoopType::OPEN_LOOP;
/** @brief The angle that was used for the FOC computation (radians)
* @note in open loop, this is the open loop angle, in closed loop, this is simply the same as the rotor position
*/
float commanded_rotor_theta = 0.0f;
/// @brief The direct and quadrature voltage vector at the time of the FOC calculation
math::direct_quad_t V_direct_quad;
/// @brief The duty cycle computation result of the frame
BldcFoc::FocDutyCycleResult duty_cycle_result;
};

/**
* @brief Get the FOC frame variables of the most recent control loop iteration
* @return The FOC frame computation of the most recent control loop iteration
*/
FOCFrameVars get_foc_frame_computation() const;
FOCController::FOCFrameVars get_foc_frame_computation() const;

~BrushlessFOCControlLoop() = default;

Expand All @@ -266,24 +202,16 @@ class BrushlessFOCControlLoop : public ControlLoop {
BrushlessFOCControlLoopParams* params_ = nullptr;
BrushlessFOCControlLoopStatus status_;

// Controllers
FOCController foc_controller_;

// Control loop state variables
utime_t last_run_time_ = 0;
float rotor_position_open_loop_start_ = 0.0f;

// Create 2 PID controllers for the Q and D currents
pid::PID<float> pid_q_current_;
pid::PID<float> pid_d_current_;

// FOC debug variables
FOCFrameVars foc_frame_vars_; // Control Loop FOC frame variables - these need to persist for the next control loop iteration
// as the ValphaBeta and VdirectQuad is used for feedforward control

/**
* @brief Run the FOC control loop
* @param foc_inputs The inputs to the FOC control loop
* @param phase_commands The phase commands to be filled in
*/
void run_foc(FOCInputs foc_inputs, hwbridge::Bridge3Phase::phase_command_t phase_commands[3]);
FOCController::FOCFrameVars
foc_frame_vars_; // Control Loop FOC frame variables - these need to persist for the next control loop iteration
// as the ValphaBeta and VdirectQuad is used for feedforward control

/**
* @brief Get the desired state of the control loop
Expand All @@ -294,13 +222,6 @@ class BrushlessFOCControlLoop : public ControlLoop {
*/
BrushlessFOCControlLoopState get_desired_state(float i_q_reference, const BrushlessFOCControlLoopState current_state);

/**
* @brief Get the desired control loop type
* @param is_any_estimator_valid Whether any of the rotor position estimators are valid
* @return The desired control loop type
*/
BrushlessFOCControlLoopType get_desired_control_loop_type(bool is_any_estimator_valid);

/**
* @brief update the rotor position estimator
* @param estimator_inputs The inputs to the rotor position estimator
Expand Down Expand Up @@ -363,11 +284,12 @@ class BrushlessFOCControlLoop : public ControlLoop {
* @param i_direct_quad_ref The direct/quadrature current reference
* @return The FOC inputs for the current control frame
*/
FOCInputs update_foc_inputs(utime_t current_time_us, utime_t last_run_time_us,
bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[],
size_t num_rotor_position_estimators, hwbridge::Bridge3Phase& bridge,
BrushlessFOCControlLoopStatus& status, math::alpha_beta_t V_alpha_beta,
const BrushlessFOCControlLoopParams* params, math::direct_quad_t i_direct_quad_ref);
FOCController::FOCInputs update_foc_inputs(utime_t current_time_us, utime_t last_run_time_us,
bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[],
size_t num_rotor_position_estimators, hwbridge::Bridge3Phase& bridge,
BrushlessFOCControlLoopStatus& status, math::alpha_beta_t V_alpha_beta,
const BrushlessFOCControlLoopParams* params,
math::direct_quad_t i_direct_quad_ref);

/*! \endcond */
};
Expand Down
Loading

0 comments on commit afb9df5

Please sign in to comment.