diff --git a/CMakeLists.txt b/CMakeLists.txt index e7136c7..e792c4a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/brushed) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/bldc) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/stepper) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util/pid) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util/math) diff --git a/control_loop/brushed/brushed_control_loop.cpp b/control_loop/brushed/brushed_control_loop.cpp index c77b26b..cd9dfc4 100644 --- a/control_loop/brushed/brushed_control_loop.cpp +++ b/control_loop/brushed/brushed_control_loop.cpp @@ -1,104 +1,158 @@ #include "brushed_control_loop.hpp" +#include "control_loop.hpp" +#include "math.h" +#include "math_util.hpp" +#include "util.hpp" + namespace control_loop { -BrushedControlLoop::BrushedControlLoop() {} +void BrushedControlLoop::init(BrushedControlLoopParams* params) { + params_ = params; + // Set the constants of the current controller + current_controller_.set_kp(params_->current_controller_params.kp); + current_controller_.set_ki(params_->current_controller_params.ki); + current_controller_.set_kd(params_->current_controller_params.kd); +} void BrushedControlLoop::run(float speed) { // Get the current time - // First check if clock is valid. If not, return early - if (!clock_) { - return; - } - - utime_t current_time_us = clock_->get_time_us(); - - // Get the motor speed from the system manager - float motor_speed = speed; + utime_t current_time = clock_.get_time_us(); + // Get the desired state + BrushedControlLoopState desired_state = + get_desired_state(speed, last_speed_, state_, current_time, deadtime_start_time_, params_->deadtime_us); + + // If the desired state is different than the current state, then we need to transition to the desired state + if (desired_state != state_) { + // If the desired state is STOP, then we need to set the deadtime start time + if (desired_state == BrushedControlLoopState::STOP) { + deadtime_start_time_ = current_time; + // Reset the current controller duty cycle + current_controller_duty_cycle_ = 0.0f; + } + // If the desired state is DEADTIME_PAUSE, then we need to set the deadtime start time + else if (desired_state == BrushedControlLoopState::DEADTIME_PAUSE) { + deadtime_start_time_ = current_time; + } + // Otherwise, we need to set the deadtime start time to 0 + else { + deadtime_start_time_ = 0; + } - // Get the brake mode from the params - if (params_ == nullptr) { - return; + // Set the state to the desired state + state_ = desired_state; } - bool brake_mode = params_->brake_mode_enabled; - - // Compute the motor speed outputs - h_bridge_motor_speed_outputs_t motor_speed_outputs = compute_motor_speed_outputs(motor_speed, brake_mode, current_time_us); - // Set the motor speed outputs - // TODO: implement this with 4 timers that can be registered - (void)motor_speed_outputs; -} + // Get the desired output + hwbridge::HBridge::HBridgeInput bridge_input = run_state(speed, state_); -BrushedControlLoop::h_bridge_motor_speed_outputs_t BrushedControlLoop::compute_motor_speed_outputs(float motor_speed, - bool brake_mode, - utime_t current_time_us) { - h_bridge_motor_speed_outputs_t motor_speed_outputs = {0.0f, 0.0f, 0.0f, 0.0f}; + // Run the bridge + bridge_.run(bridge_input); - if (params_ == nullptr) { - return motor_speed_outputs; - } + // Update the last speed + last_speed_ = speed; +} - // Check if the motor speed is 0.0f - if (motor_speed == 0.0f) { - // Check if brake mode is enabled - if (brake_mode) { - // Set the duty cycle of the low pins to 1.0f and the high pins to 0.0f - motor_speed_outputs.DC_A_HIGH = 0.0f; - motor_speed_outputs.DC_A_LOW = 1.0f; - motor_speed_outputs.DC_B_HIGH = 0.0f; - motor_speed_outputs.DC_B_LOW = 1.0f; - - } else { - // Set the duty cycle of all pins to 0.0f - motor_speed_outputs.DC_A_HIGH = 0.0f; - motor_speed_outputs.DC_A_LOW = 0.0f; - motor_speed_outputs.DC_B_HIGH = 0.0f; - motor_speed_outputs.DC_B_LOW = 0.0f; +BrushedControlLoop::BrushedControlLoopState BrushedControlLoop::get_desired_state(float desired_speed, float previous_speed, + BrushedControlLoopState current_state, + utime_t current_time, + utime_t deadtime_start_time, + utime_t deadtime_pause_time_us) { + BrushedControlLoopState desired_state = current_state; + + switch (current_state) { + case BrushedControlLoopState::STOP: + if (desired_speed != 0) { + // NOTE: we should check that we are in the stop state for at least DEADTIME_PAUSE_TIME_US before transitioning to + // RUN + desired_state = BrushedControlLoopState::RUN; + } + break; + case BrushedControlLoopState::RUN: { + if (fabs(desired_speed) < math::ACCEPTABLE_FLOAT_ERROR) { + desired_state = BrushedControlLoopState::STOP; + } + // Otherwise, if the desired speed is different than the previous speed, then we need to pause the control loop for + // deadtime + else if ((desired_speed * previous_speed < 0) && (deadtime_pause_time_us > 0)) { + // If the deadtime pause ticks is > 0, then we need to pause the control loop for deadtime + desired_state = BrushedControlLoopState::DEADTIME_PAUSE; + } else { + // Do nothing, maintain the RUN state + desired_state = BrushedControlLoopState::RUN; + } + } break; + case BrushedControlLoopState::DEADTIME_PAUSE: { + // If the speed is 0, then we can transition to the STOP state + if (fabs(desired_speed) < math::ACCEPTABLE_FLOAT_ERROR) { + desired_state = BrushedControlLoopState::STOP; + } + // Otherwise, check to see if the deadtime has expired + else if ((current_time - deadtime_start_time) >= deadtime_pause_time_us) { + desired_state = BrushedControlLoopState::RUN; + } else { + // Do nothing, maintain the DEADTIME_PAUSE state + desired_state = BrushedControlLoopState::DEADTIME_PAUSE; + } } - } else if (motor_speed >= 0.0f) { - // A high and B low should be set to a scale of absolute value of motor speed from 0.0f to 1.0f - // using the max motor speed constant - motor_speed_outputs.DC_A_HIGH = motor_speed / MAX_MOTOR_SPEED; - motor_speed_outputs.DC_A_LOW = 0.0f; - motor_speed_outputs.DC_B_HIGH = 0.0f; - motor_speed_outputs.DC_B_LOW = motor_speed / MAX_MOTOR_SPEED; - } else if (motor_speed < 0.0f) { - // A low and B high should be set to a scale of absolute value of motor speed from 0.0f to 1.0f - // using the max motor speed constant - motor_speed_outputs.DC_A_HIGH = 0.0f; - motor_speed_outputs.DC_A_LOW = -motor_speed / MAX_MOTOR_SPEED; - motor_speed_outputs.DC_B_HIGH = -motor_speed / MAX_MOTOR_SPEED; - motor_speed_outputs.DC_B_LOW = 0.0f; + default: + break; } - // Dead time insertion- if the signs of the last successful motor speed and the current motor speed are different - // and the time is less than the last speed change time plus the dead time constant, then set the duty cycle of all - // pins to 0.0f - - // First check if the signs are different - if ((motor_speed * last_motor_speed_ < 0.0f)) { - // Update the last changed time - last_speed_dir_change_time_us_ = current_time_us; - } + return desired_state; +} - // Check if the time is less than the last speed change time plus the dead time constant - if (current_time_us < params_->h_bridge_dead_time_us + last_speed_dir_change_time_us_) { - // Set the duty cycle of all pins to 0.0f - motor_speed_outputs.DC_A_HIGH = 0.0f; - motor_speed_outputs.DC_A_LOW = 0.0f; - motor_speed_outputs.DC_B_HIGH = 0.0f; - motor_speed_outputs.DC_B_LOW = 0.0f; - } +hwbridge::HBridge::HBridgeInput BrushedControlLoop::run_state(float speed, const BrushedControlLoopState& state) { + hwbridge::HBridge::HBridgeInput bridge_input = {0.0f, false, 0.0f, false}; + + switch (state) { + case BrushedControlLoopState::STOP: { + switch (params_->brake_mode) { + case BrushedBrakeType::COAST: { + bridge_input = {0.0f, false, 0.0f, false}; + }; break; + case BrushedBrakeType::BRAKE_LOW_SIDE: { + bridge_input = {0.0f, true, 0.0f, true}; + }; break; + case BrushedBrakeType::BRAKE_HIGH_SIDE: { + bridge_input = {1.0f, false, 1.0f, false}; + }; break; + default: + // do nothing + break; + }; + } break; + case BrushedControlLoopState::RUN: { + // If the speed is positive, then we need to drive the motor forward (A high, B low) + // If the speed is negative, then we need to drive the motor backward (A low, B high) + if (speed > 0) { + bridge_input = {speed, false, 0.0f, true}; + } else { + bridge_input = {0.0f, true, -speed, false}; + } + } break; + case BrushedControlLoopState::DEADTIME_PAUSE: { + // Set the bridge to float + bridge_input = {0.0f, false, 0.0f, false}; + } break; + default: + // Do nothing + break; + }; + + return bridge_input; +} - last_motor_speed_ = motor_speed; +// Write the run_constant_current function +void BrushedControlLoop::run_constant_current(float current) { + // Get the current bridge current + float bridge_current = 0.0f; + bridge_.get_current(bridge_current); - return motor_speed_outputs; -} + // Run the current controller + current_controller_duty_cycle_ += current_controller_.calculate(current, bridge_current); -void BrushedControlLoop::init(BrushedControlLoopParams* params) { - params_ = params; - last_speed_dir_change_time_us_ = 0; + run(current_controller_duty_cycle_); } } // namespace control_loop \ No newline at end of file diff --git a/control_loop/brushed/brushed_control_loop.hpp b/control_loop/brushed/brushed_control_loop.hpp index 3b86fa3..1dc1b6a 100644 --- a/control_loop/brushed/brushed_control_loop.hpp +++ b/control_loop/brushed/brushed_control_loop.hpp @@ -2,12 +2,10 @@ #define BRUSHED_CONTROL_LOOP_HPP #include +#include "bridge_hbridge.hpp" #include "control_loop.hpp" #include "hal_clock.hpp" -#ifdef UNIT_TEST -// forward declare the BrushedControlLoopTest class -class BrushedControlLoopTest; -#endif +#include "pid.hpp" namespace control_loop { @@ -15,58 +13,84 @@ namespace control_loop { class BrushedControlLoop : public ControlLoop { public: - BrushedControlLoop(); + BrushedControlLoop(hwbridge::HBridge& bridge, basilisk_hal::HAL_CLOCK& clock) + : bridge_(bridge), clock_(clock), current_controller_{0, 0, 0, 1.0f, -1.0f, 0} {} + + class BrushedControlLoopCurrentControllerParams { + public: + float kp; // The proportional gain for the current control loop + float ki; // The integral gain for the current control loop + float kd; // The derivative gain for the current control loop + }; + + enum class BrushedBrakeType { + COAST, + BRAKE_LOW_SIDE, + BRAKE_HIGH_SIDE, + }; - // Define a class for the brushed motor params class BrushedControlLoopParams { public: - bool brake_mode_enabled; // Whether brake mode is enabled or not (TODO: Make this an enum between high side or low side) - utime_t h_bridge_dead_time_us; // Amount of time to wait between switching the high and low pins of the H bridge + BrushedBrakeType brake_mode; // Whether or not to brake the motor when the control loop is not running + float deadtime_us; // The deadtime to apply to the h bridge to prevet shoot through + BrushedControlLoopCurrentControllerParams current_controller_params; + }; + + enum class BrushedControlLoopState { + STOP, + RUN, + DEADTIME_PAUSE, }; /** - * @brief Initialize the control loop with the params - * @param params Pointer to the params + * @brief Initialize the control loop + * @param params The parameters for the control loop + * @return void */ void init(BrushedControlLoopParams* params); + void run(float speed) override; + /** - * @brief Run the control loop - * @param speed The desired speed of the motor (-1.0f->1.0f) + * @brief Run the motor at a constant current + * @param current The current to run the motor at + * @return void */ - void run(float speed) override; + void run_constant_current(float current); - // Function to register a timer with the control loop + protected: + hwbridge::HBridge& bridge_; + basilisk_hal::HAL_CLOCK& clock_; + BrushedControlLoopParams* params_ = nullptr; - // Function to register a HAL clock with the control loop - void register_clock(basilisk_hal::HAL_CLOCK* clock) { clock_ = clock; } + BrushedControlLoopState state_ = BrushedControlLoopState::STOP; + float last_speed_ = 0.0f; + utime_t deadtime_start_time_ = 0; - // 0.0 -> 1.0f - typedef struct h_bridge_motor_speed_outputs_t { - float DC_A_HIGH; - float DC_A_LOW; - float DC_B_HIGH; - float DC_B_LOW; - } h_bridge_motor_speed_outputs_t; + // Define a PID controller for the current control loop + pid::PID current_controller_; + // Define a variable to store the duty cycle commanded by the current controller + float current_controller_duty_cycle_ = 0.0f; - private: /** - * @brief Computes the PWM duty cycle of the 4 pins based on desired motor speed (-1.0f->1.0f) - * and whether brake mode is enabled or not - * - * @param motor_speed - * @param brake_mode + * @brief Get the desired state of the control loop + * @param desired_speed The desired speed of the motor + * @param previous_speed The previous speed of the motor + * @param current_state The current state of the control loop + * @param current_time The current time + * @param deadtime_start_time The time at which the deadtime started + * @param deadtime_pause_time_us The number of ticks to pause the control loop for during deadtime + * @return The desired state of the control loop */ - h_bridge_motor_speed_outputs_t compute_motor_speed_outputs(float motor_speed, bool brake_mode, utime_t current_time_us); + BrushedControlLoopState get_desired_state(float desired_speed, float previous_speed, BrushedControlLoopState current_state, + utime_t current_time, utime_t deadtime_start_time, utime_t deadtime_pause_time_us); - utime_t last_speed_dir_change_time_us_{0}; - float last_motor_speed_{0.0f}; - basilisk_hal::HAL_CLOCK* clock_{nullptr}; - BrushedControlLoopParams* params_{nullptr}; - -#ifdef UNIT_TEST - friend class BrushedControlLoopTest; -#endif + /** + * @brief run and get the bridge input for a given speed and state + * @param speed The speed to run the motor at + * @param state The state of the control loop + */ + hwbridge::HBridge::HBridgeInput run_state(float speed, const BrushedControlLoopState& state); }; } // namespace control_loop diff --git a/control_loop/control_loop.hpp b/control_loop/control_loop.hpp index a364c57..6f9bfdd 100644 --- a/control_loop/control_loop.hpp +++ b/control_loop/control_loop.hpp @@ -9,7 +9,7 @@ class ControlLoop { public: virtual void run(float speed) = 0; // Define a max motor_speed member constant - constexpr static float MAX_MOTOR_SPEED = 1.0f; + static constexpr float MAX_MOTOR_SPEED = 1.0f; }; } // namespace control_loop diff --git a/control_loop/stepper/stepper_control_loop.cpp b/control_loop/stepper/stepper_control_loop.cpp index c9318ba..20550f4 100644 --- a/control_loop/stepper/stepper_control_loop.cpp +++ b/control_loop/stepper/stepper_control_loop.cpp @@ -1,83 +1,51 @@ #include "stepper_control_loop.hpp" #include "math.h" +#include "math_util.hpp" +#include "util.hpp" namespace control_loop { -// Run the stepper control loop -void StepperControlLoop::run(float speed) { - // Get the current time - utime_t current_time = clock_.get_time_us(); - // Edge case: If this is the first time this function is called, set the previous time to the current time - if (previous_time_ == 0) { - previous_time_ = current_time; - } - // Get the desired speed from the system manager - float desired_speed = speed; - - // Determine the time delta since the last time this function was called - float time_delta = (current_time - previous_time_) / basilisk_hal::HAL_CLOCK::kMicrosecondsPerSecond; - float desired_angle = previous_angle_; - - if (params_->stepper_motor_simple_switcher_enabled) { - // Call the determineElectricalAngleSimple function to determine the electrical angle of the motor - desired_angle = determineElectricalAngleSimple(time_delta, desired_speed, previous_angle_); - } else { - // Determine the electrical angle of the motor - desired_angle = determineElectricalAngleSimple(time_delta, desired_speed, previous_angle_); - // Update the previous time and angle - previous_time_ = clock_.get_time_us(); - previous_angle_ = desired_angle; - } - - // Determine the A and B current scalars based on the electrical angle - std::pair current_scalars = determineCurrentSetpointScalars(desired_angle); - - if (params_->stepper_motor_disable_current_pid) { - // Set the current setpoints of the motors - motor_a_.run(params_->stepper_motor_current_to_pwm_duty_cycle_slope * current_scalars.first); - motor_b_.run(params_->stepper_motor_current_to_pwm_duty_cycle_slope * current_scalars.second); - } -} +void StepperControlLoop::init(StepperControlLoopParams* params) { + params_ = params; -// Determine the electrical angle of the motor -float StepperControlLoop::determineElectricalAngle(float time_delta, float desired_speed, float previous_angle) { - // determine the angle based on the desired speed and the time delta - // A step is 360 degrees / 4 blocks = 90 degrees - float angle = previous_angle + (desired_speed * time_delta) * 360.0 / 4; - // If the angle is greater than 360, subtract 360 - if (angle > 360.0f) { - angle -= 360.0f; - } - // If the angle is less than 0, add 360 - if (angle < 0.0f) { - angle += 360.0f; - } - return angle; + // Set the previous time to the current time + previous_time_ = clock_.get_time_us(); } -float StepperControlLoop::determineElectricalAngleSimple(float time_delta, float desired_speed, float previous_angle) { - // call the determineElectricalAngle function - float angle = determineElectricalAngle(time_delta, desired_speed, previous_angle); - // calculate the difference between the angle and the previous angle - float angle_diff = angle - previous_angle; - // If the angle difference is greater than 90 degrees, return that angle. Otherwise, return the previous angle - if (fabs(angle_diff) > 90.0f) { - previous_time_ = clock_.get_time_us(); - previous_angle_ = angle; - return angle; - } else { - return previous_angle; - } +// Run the stepper control loop +void StepperControlLoop::run(float speed) { + do { + if (params_ == nullptr) { + // If the params are not set, return + break; + } + // First, integrate the electrical angle based on the speed + const float dt = (clock_.get_time_us() - previous_time_) / clock_.kMicrosecondsPerSecond; + electrical_angle_ += speed * params_->max_speed * dt; + // Wrap the electrical angle around 0 and 2pi + math::wraparound(electrical_angle_, 0.0f, 2.0f * math::M_PI_FLOAT); + + // If the fabs speed is 0, then we are not moving, so set the current setpoints to the hold current + const float current_setpoint = (fabs(speed) < math::ACCEPTABLE_FLOAT_ERROR) ? params_->i_hold : params_->i_run; + + // Determine the current setpoints + auto current_setpoint_scalars = determine_current_setpoints(current_setpoint, electrical_angle_); + + // Run the current controllers of the 2 brushed control loops + bridge_a_.run_constant_current(current_setpoint_scalars.first); + bridge_b_.run_constant_current(current_setpoint_scalars.second); + + } while (false); } -std::pair StepperControlLoop::determineCurrentSetpointScalars(float electrical_angle) { +std::pair StepperControlLoop::determine_current_setpoints(float desired_current, float electrical_angle) { // Determine the A and B current scalars based on the electrical angle // The A current scalar is the cosine of the electrical angle // The B current scalar is the sine of the electrical angle - float a_current_scalar = cos(electrical_angle * M_PI / 180.0f); - float b_current_scalar = sin(electrical_angle * M_PI / 180.0f); - return std::make_pair(a_current_scalar, b_current_scalar); + float a_current_scalar = cos(electrical_angle); + float b_current_scalar = sin(electrical_angle); + return std::make_pair(a_current_scalar * desired_current, b_current_scalar * desired_current); } } // namespace control_loop \ No newline at end of file diff --git a/control_loop/stepper/stepper_control_loop.hpp b/control_loop/stepper/stepper_control_loop.hpp index f156429..c03ef90 100644 --- a/control_loop/stepper/stepper_control_loop.hpp +++ b/control_loop/stepper/stepper_control_loop.hpp @@ -2,9 +2,10 @@ #define STEPPER_CONTROL_LOOP_HPP #include -#include "bridge_hbridge.hpp" +#include "brushed_control_loop.hpp" #include "control_loop.hpp" #include "hal_clock.hpp" +#include "pid.hpp" namespace control_loop { @@ -12,51 +13,45 @@ class StepperControlLoop : public ControlLoop { public: class StepperControlLoopParams { public: - float stepper_motor_current_to_pwm_duty_cycle_slope; - bool stepper_motor_disable_current_pid; - bool stepper_motor_simple_switcher_enabled; - }; + float i_hold; // The current to hold the motor at when not moving at the specific electrical angle + float i_run; // The current to run the motor at when moving at the specific electrical angle - // Define a constructor that takes 2 references to HBridge objects. One for the A motor and one for the B motor. Also, a clock - // instance - StepperControlLoop(hwbridge::HBridge& motor_a, hwbridge::HBridge& motor_b, basilisk_hal::HAL_CLOCK& clock) - : motor_a_(motor_a), motor_b_(motor_b), clock_(clock) {} - void run(float speed) override; + float max_speed; // The maximum electrical speed of the motor (radians / second) + // NOTE: the maximum speed is not the same as the maximum mechanical speed, which is the maximum electrical speed divided + // by the number of pole pairs + }; - // Define a function to determine the electrical angle of the motor /** - * @brief Determine the electrical angle of the motor - * @param time_delta The time delta since the last time this function was called in seconds - * @param desired_speed The desired speed of the motor - * @param previous_angle The previous angle of the motor - * - * @return The new angle of the motor in degrees (0 -> 360) + * @brief Construct a new StepperControlLoop object + * @param bridge_a The HBridge object for the A motor + * @param bridge_b The HBridge object for the B motor + * @param clock The HAL_CLOCK object + * @return void */ - float determineElectricalAngle(float time_delta, float desired_speed, float previous_angle); + StepperControlLoop(BrushedControlLoop& bridge_a, BrushedControlLoop& bridge_b, basilisk_hal::HAL_CLOCK& clock) + : bridge_a_(bridge_a), bridge_b_(bridge_b), clock_(clock) {} - /** - * @brief Determine the electrical angle of the motor using 4 step commutation - * @param time_delta The time delta since the last time this function was called in seconds - * @param desired_speed The desired speed of the motor - * @param previous_angle The previous angle of the motor - */ - float determineElectricalAngleSimple(float time_delta, float desired_speed, float previous_angle); + void init(StepperControlLoopParams* params); + + void run(float speed); - // Define a function to determine the AB and CD current magnitudes based on the electrical angle + protected: /** - * @brief Determine the A and B current scalar (-1 -> 1) based on the electrical angle - * @param electrical_angle The electrical angle of the motor - * @return A pair of floats. The first is the A current scalar and the second is the B current scalar + * @brief determine the current setpoint scalars based on the electrical angle + * @param electrical_angle The electrical angle of the motor (radians) + * @return std::pair The A and B current scalars */ - std::pair determineCurrentSetpointScalars(float electrical_angle); + std::pair determine_current_setpoints(float desired_current, float electrical_angle); - private: - hwbridge::HBridge& motor_a_; - hwbridge::HBridge& motor_b_; + BrushedControlLoop& bridge_a_; + BrushedControlLoop& bridge_b_; basilisk_hal::HAL_CLOCK& clock_; - float previous_angle_ = 0.0f; + float electrical_angle_ = 0; // The electrical angle of the motor (radians) uint32_t previous_time_ = 0; StepperControlLoopParams* params_ = nullptr; + + // Create 2 variables to store the current setpoints for the A and B motors + float current_setpoint_a, current_setpoint_b = 0; }; } // namespace control_loop diff --git a/hwbridge/h_bridge/bridge_hbridge.hpp b/hwbridge/h_bridge/bridge_hbridge.hpp index 53463b0..347d6e3 100644 --- a/hwbridge/h_bridge/bridge_hbridge.hpp +++ b/hwbridge/h_bridge/bridge_hbridge.hpp @@ -1,5 +1,8 @@ #ifndef BRIDGE_HBRIDGE_HPP #define BRIDGE_HBRIDGE_HPP +#include "hal_common.hpp" +#include "math.h" +#include "math_util.hpp" namespace hwbridge { @@ -9,8 +12,45 @@ class HBridge { HBridge() = default; virtual ~HBridge() = default; - // Define a virtual function to run the h-bridge - virtual void run(float speed) = 0; + class HBridgeInput { + public: + float duty_cycle_a_h; + bool low_side_a_gpio_state; + float duty_cycle_b_h; + bool low_side_b_gpio_state; + + // Overload the == operator + bool operator==(const HBridgeInput& other) const { + const bool low_side_gpios_equal = + (low_side_a_gpio_state == other.low_side_a_gpio_state) && (low_side_b_gpio_state == other.low_side_b_gpio_state); + // Use the very small number for floating comparisons + const bool duty_cycles_equal = (fabs(duty_cycle_a_h - other.duty_cycle_a_h) < math::ACCEPTABLE_FLOAT_ERROR) && + (fabs(duty_cycle_b_h - other.duty_cycle_b_h) < math::ACCEPTABLE_FLOAT_ERROR); + + return low_side_gpios_equal && duty_cycles_equal; + } + }; + + /** + * @brief Run the HBridge at a given speed + * @param speed The speed to run the HBridge at + * @return app_hal_status_E The status of the operation + */ + virtual app_hal_status_E run(HBridgeInput input) = 0; + + /** + * @brief Get the current through the HBridge + * @param current The current through the HBridge + * @return app_hal_status_E The status of the operation + */ + virtual app_hal_status_E get_current(float& current) = 0; + + /** + * @brief Get the voltage across the HBridge + * @param voltage The voltage across the HBridge + * @return app_hal_status_E The status of the operation + */ + virtual app_hal_status_E get_voltage(float& voltage) = 0; }; } // namespace hwbridge diff --git a/hwbridge/h_bridge/bridge_hbridge_drv8801.hpp b/hwbridge/h_bridge/bridge_hbridge_drv8801.hpp index 26b061c..567dc59 100644 --- a/hwbridge/h_bridge/bridge_hbridge_drv8801.hpp +++ b/hwbridge/h_bridge/bridge_hbridge_drv8801.hpp @@ -6,6 +6,7 @@ #include "control_loop.hpp" #include "hal_gpio.hpp" #include "hal_timer.hpp" +#include "math_util.hpp" namespace hwbridge { @@ -36,9 +37,12 @@ class HBridgeDRV8801 : public HBridge { } // Define a run method that overrides the run method in HBridge - void run(float speed) override { - // Set the direction pin - dir_pin_->set_output_state(speed > 0); + app_hal_status_E run(HBridgeInput input) override { + // Set the direction pin based on which low side GPIO is active + dir_pin_->set_output_state(input.low_side_b_gpio_state); + + // Determine the speed based on the highest duty cycle of the 2 high side timers + float speed = math::max(input.duty_cycle_a_h, input.duty_cycle_b_h); // Determine the appropriate amount of ticks by getting the timer period uint32_t period = pwm_timer_->get_period(); @@ -46,6 +50,8 @@ class HBridgeDRV8801 : public HBridge { // Set the PWM timer pwm_timer_->set_channel(ticks, channel_); + + return app_hal_status_E::APP_HAL_OK; } private: diff --git a/scripts/setup.sh b/scripts/setup.sh index 07ad373..9c55cec 100644 --- a/scripts/setup.sh +++ b/scripts/setup.sh @@ -2,12 +2,8 @@ set -euxo pipefail -sudo apt install git clang-format clang-tidy +sudo apt install git clang-format clang-tidy build-essential git submodule update --init --recursive mkdir -p build/ > /dev/null - -cd build - -cmake .. -DCMAKE_BUILD_TYPE=Debug -G "Unix Makefiles" \ No newline at end of file diff --git a/test/brushed_control_loop_test.cpp b/test/brushed_control_loop_test.cpp index 748fcf4..80e3a90 100644 --- a/test/brushed_control_loop_test.cpp +++ b/test/brushed_control_loop_test.cpp @@ -2,158 +2,362 @@ #define BRUSHED_CONTROL_LOOP_TEST_HPP #include "brushed_control_loop.hpp" +#include "bridge_hbridge.hpp" #include "gmock/gmock.h" +#include "mock_hal_clock.hpp" +#include "mock_hbridge.hpp" namespace control_loop { +// Make a class to extend the BrushedControlLoop class so we can access its protected members +class BrushedControlLoopTest : public BrushedControlLoop { + public: + BrushedControlLoopParams default_params{ + .brake_mode = BrushedBrakeType::COAST, + .deadtime_us = 0, + .current_controller_params = + { + .kp = 0.0f, + .ki = 0.0f, + .kd = 0.0f, + }, + }; + + hwbridge::MOCK_HBRIDGE mock_bridge_; + basilisk_hal::MOCK_HAL_CLOCK mock_clock_; + + BrushedControlLoopTest() : BrushedControlLoop(mock_bridge_, mock_clock_) {} + + using BrushedControlLoop::get_desired_state; + using BrushedControlLoop::run_state; + using BrushedControlLoop::state_; +}; using namespace ::testing; -class BrushedControlLoopTest { - public: - BrushedControlLoopTest() : brushed_control_loop_() {} +TEST(BrushedControlLoopTest, test_stop_to_run) { + // Test that a control loop in the STOP state can transition to the RUN state if the speed is 0 + BrushedControlLoopTest control_loop; + control_loop.init(&control_loop.default_params); + utime_t deadband_start_time = 0; + EXPECT_EQ( + control_loop.get_desired_state(0.1, 0.0, BrushedControlLoop::BrushedControlLoopState::STOP, 0, deadband_start_time, 0), + BrushedControlLoop::BrushedControlLoopState::RUN); +} - // Define a getter method for the brushed_control_loop_ member variable that returns a reference - BrushedControlLoop& get_brushed_control_loop() { return brushed_control_loop_; } +// Test RUN to STOP +TEST(BrushedControlLoopTest, test_run_to_stop) { + // Test that a control loop in the RUN state can transition to the STOP state if the speed is 0 + BrushedControlLoopTest control_loop; + control_loop.init(&control_loop.default_params); + utime_t deadband_start_time = 0; + EXPECT_EQ( + control_loop.get_desired_state(0.0, 0.1, BrushedControlLoop::BrushedControlLoopState::RUN, 0, deadband_start_time, 0), + BrushedControlLoop::BrushedControlLoopState::STOP); +} - // Define an accessor method for the compute_motor_speed_outputs method - BrushedControlLoop::h_bridge_motor_speed_outputs_t compute_motor_speed_outputs(float motor_speed, bool brake_mode, - utime_t current_time_us) { - return brushed_control_loop_.compute_motor_speed_outputs(motor_speed, brake_mode, current_time_us); - } +// Test that a RUN from a speed of 0.1 -> -0.1 does NOT transition to DEADTIME_PAUSE if the deadtime_pause_time_us is 0 +TEST(BrushedControlLoopTest, test_run_to_run_without_deadtime) { + // Test that a control loop in the RUN state can transition to the DEADTIME_PAUSE state if the speed is 0 + BrushedControlLoopTest control_loop; + control_loop.init(&control_loop.default_params); + utime_t deadband_start_time = 0; + EXPECT_EQ( + control_loop.get_desired_state(-0.1, 0.1, BrushedControlLoop::BrushedControlLoopState::RUN, 0, deadband_start_time, 0), + BrushedControlLoop::BrushedControlLoopState::RUN); +} - utime_t& get_last_speed_dir_change_time_us() { return brushed_control_loop_.last_speed_dir_change_time_us_; } +// Test that a RUN from a speed of 0.1 -> -0.1 DOES transition to DEADTIME_PAUSE if the deadtime_pause_time_us is 1 +TEST(BrushedControlLoopTest, test_run_to_deadtime_pause_with_deadtime) { + // Test that a control loop in the RUN state can transition to the DEADTIME_PAUSE state if the speed is 0 + BrushedControlLoopTest control_loop; + control_loop.init(&control_loop.default_params); + utime_t deadband_start_time = 0; + EXPECT_EQ( + control_loop.get_desired_state(-0.1, 0.1, BrushedControlLoop::BrushedControlLoopState::RUN, 0, deadband_start_time, 1), + BrushedControlLoop::BrushedControlLoopState::DEADTIME_PAUSE); +} - void init(BrushedControlLoop::BrushedControlLoopParams* params) { brushed_control_loop_.init(params); } +// Test that we can transition from DEADTIME_PAUSE to RUN if the deadtime has expired +TEST(BrushedControlLoopTest, test_deadtime_pause_to_run) { + // Test that a control loop in the RUN state can transition to the DEADTIME_PAUSE state if the speed is 0 + BrushedControlLoopTest control_loop; + control_loop.init(&control_loop.default_params); + utime_t deadband_start_time = 0; + EXPECT_EQ(control_loop.get_desired_state(-0.1, 0.1, BrushedControlLoop::BrushedControlLoopState::DEADTIME_PAUSE, 0, + deadband_start_time, 1), + BrushedControlLoop::BrushedControlLoopState::DEADTIME_PAUSE); + EXPECT_EQ(control_loop.get_desired_state(-0.1, 0.1, BrushedControlLoop::BrushedControlLoopState::DEADTIME_PAUSE, 1, + deadband_start_time, 1), + BrushedControlLoop::BrushedControlLoopState::RUN); +} - private: - BrushedControlLoop brushed_control_loop_; -}; +// Test that we can transition from DEADTIME_PAUSE to STOP if the speed is 0 +TEST(BrushedControlLoopTest, test_deadtime_pause_to_stop) { + // Test that a control loop in the RUN state can transition to the DEADTIME_PAUSE state if the speed is 0 + BrushedControlLoopTest control_loop; + control_loop.init(&control_loop.default_params); + utime_t deadband_start_time = 0; + EXPECT_EQ(control_loop.get_desired_state(0.0, 0.1, BrushedControlLoop::BrushedControlLoopState::DEADTIME_PAUSE, 0, + deadband_start_time, 1), + BrushedControlLoop::BrushedControlLoopState::STOP); +} + +// Test the run state function with a speed of 0, and a brake mode of COAST, then BRAKE_LOW_SIDE, then BRAKE_HIGH_SIDE +TEST(BrushedControlLoopTest, test_run_state_stop) { + BrushedControlLoopTest control_loop; + BrushedControlLoop::BrushedControlLoopParams params{ + .brake_mode = BrushedControlLoop::BrushedBrakeType::COAST, + .deadtime_us = 0, + .current_controller_params = + { + .kp = 0.0f, + .ki = 0.0f, + .kd = 0.0f, + }, + }; + + control_loop.init(¶ms); + control_loop.state_ = BrushedControlLoop::BrushedControlLoopState::STOP; -// Test the compute_motor_speed_outputs method -// Test that a motor speed of 0.0f with brake mode false returns a duty cycle of 0.0f for all pins -TEST(BrushedControlLoopTests, compute_motor_speed_outputs_zero_speed_no_brake) { - BrushedControlLoopTest brushed_control_loop; - control_loop::BrushedControlLoop::BrushedControlLoopParams params{ - .brake_mode_enabled = false, - .h_bridge_dead_time_us = 0, + hwbridge::HBridge::HBridgeInput expected_output{ + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = false, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = false, }; - brushed_control_loop.init(¶ms); + // Get the actual output + hwbridge::HBridge::HBridgeInput actual_output = control_loop.run_state(0.0f, control_loop.state_); + + // Compare the actual output to the expected output + EXPECT_EQ(actual_output.duty_cycle_a_h, expected_output.duty_cycle_a_h); + EXPECT_EQ(actual_output.low_side_a_gpio_state, expected_output.low_side_a_gpio_state); + EXPECT_EQ(actual_output.duty_cycle_b_h, expected_output.duty_cycle_b_h); + EXPECT_EQ(actual_output.low_side_b_gpio_state, expected_output.low_side_b_gpio_state); + + // Set the params to BRAKE_LOW_SIDE + params.brake_mode = BrushedControlLoop::BrushedBrakeType::BRAKE_LOW_SIDE; + + // Reinitialize the control loop + control_loop.init(¶ms); + + // Get the actual output + actual_output = control_loop.run_state(0.0f, control_loop.state_); + + // Set the expected output low side gpio's to true + expected_output.low_side_a_gpio_state = true; + expected_output.low_side_b_gpio_state = true; + + // Compare the actual output to the expected output + EXPECT_EQ(actual_output.duty_cycle_a_h, expected_output.duty_cycle_a_h); + EXPECT_EQ(actual_output.low_side_a_gpio_state, expected_output.low_side_a_gpio_state); + EXPECT_EQ(actual_output.duty_cycle_b_h, expected_output.duty_cycle_b_h); + EXPECT_EQ(actual_output.low_side_b_gpio_state, expected_output.low_side_b_gpio_state); + + // Set the params to BRAKE_HIGH_SIDE + params.brake_mode = BrushedControlLoop::BrushedBrakeType::BRAKE_HIGH_SIDE; - BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = - brushed_control_loop.compute_motor_speed_outputs(0.0f, false, params.h_bridge_dead_time_us); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 0.0f); + // Reinitialize the control loop + control_loop.init(¶ms); + + // Get the actual output + actual_output = control_loop.run_state(0.0f, control_loop.state_); + + // Set the expected output high side duty cycles to 1.0, and the low side gpio's to false + expected_output.duty_cycle_a_h = 1.0f; + expected_output.duty_cycle_b_h = 1.0f; + expected_output.low_side_a_gpio_state = false; + expected_output.low_side_b_gpio_state = false; + + // Compare the actual output to the expected output + EXPECT_EQ(actual_output.duty_cycle_a_h, expected_output.duty_cycle_a_h); + EXPECT_EQ(actual_output.low_side_a_gpio_state, expected_output.low_side_a_gpio_state); + EXPECT_EQ(actual_output.duty_cycle_b_h, expected_output.duty_cycle_b_h); + EXPECT_EQ(actual_output.low_side_b_gpio_state, expected_output.low_side_b_gpio_state); } -// Test that a motor speed of 0.0f with brake mode true returns a duty cycle of 1.0f for low pins and 0.0f for high pins -TEST(BrushedControlLoopTests, compute_motor_speed_outputs_zero_speed_brake) { - BrushedControlLoopTest brushed_control_loop; +// Test the run state function with a state of DEADTIME_PAUSE and a speed of -0.3 +// The bridge should just be floating +TEST(BrushedControlLoopTest, test_run_state_deadtime_pause) { + BrushedControlLoopTest control_loop; + BrushedControlLoop::BrushedControlLoopParams params{ + .brake_mode = BrushedControlLoop::BrushedBrakeType::BRAKE_HIGH_SIDE, + .deadtime_us = 0, + .current_controller_params = + { + .kp = 0.0f, + .ki = 0.0f, + .kd = 0.0f, + }, + }; + + control_loop.init(¶ms); + control_loop.state_ = BrushedControlLoop::BrushedControlLoopState::DEADTIME_PAUSE; - control_loop::BrushedControlLoop::BrushedControlLoopParams params{ - .brake_mode_enabled = true, - .h_bridge_dead_time_us = 0, + hwbridge::HBridge::HBridgeInput expected_output{ + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = false, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = false, }; - brushed_control_loop.init(¶ms); + // Get the actual output + hwbridge::HBridge::HBridgeInput actual_output = control_loop.run_state(0.0f, control_loop.state_); - BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = - brushed_control_loop.compute_motor_speed_outputs(0.0f, true, params.h_bridge_dead_time_us); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 1.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 1.0f); + // Compare the actual output to the expected output + EXPECT_EQ(actual_output.duty_cycle_a_h, expected_output.duty_cycle_a_h); + EXPECT_EQ(actual_output.low_side_a_gpio_state, expected_output.low_side_a_gpio_state); + EXPECT_EQ(actual_output.duty_cycle_b_h, expected_output.duty_cycle_b_h); + EXPECT_EQ(actual_output.low_side_b_gpio_state, expected_output.low_side_b_gpio_state); } -// Test that a motor speed of 1.0f returns a duty cycle of 1.0f A high and B low and 0.0f for the other pins -TEST(BrushedControlLoopTests, compute_motor_speed_outputs_full_speed) { - BrushedControlLoopTest brushed_control_loop; +// Test the run state function with a state of RUN and a speed of 0.3. +// Expect the A duty cycle to be 0.3, and the B duty cycle to be 0.0 +// Expect A low side gpio to be false, and B low side gpio to be true +TEST(BrushedControlLoopTest, test_run_positive_dir) { + BrushedControlLoopTest control_loop; - BrushedControlLoop::BrushedControlLoopParams params{ - .brake_mode_enabled = false, - .h_bridge_dead_time_us = 0, + control_loop.init(&control_loop.default_params); + control_loop.state_ = BrushedControlLoop::BrushedControlLoopState::RUN; + + hwbridge::HBridge::HBridgeInput expected_output{ + .duty_cycle_a_h = 0.3f, + .low_side_a_gpio_state = false, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = true, }; - brushed_control_loop.init(¶ms); + // Get the actual output + hwbridge::HBridge::HBridgeInput actual_output = control_loop.run_state(0.3f, control_loop.state_); - BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( - control_loop::ControlLoop::MAX_MOTOR_SPEED, false, params.h_bridge_dead_time_us); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 1.0f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 1.0f); + // Compare the actual output to the expected output + EXPECT_EQ(actual_output.duty_cycle_a_h, expected_output.duty_cycle_a_h); + EXPECT_EQ(actual_output.low_side_a_gpio_state, expected_output.low_side_a_gpio_state); + EXPECT_EQ(actual_output.duty_cycle_b_h, expected_output.duty_cycle_b_h); + EXPECT_EQ(actual_output.low_side_b_gpio_state, expected_output.low_side_b_gpio_state); } -// Test that a motor speed of -1.0f returns a duty cycle of 1.0f A low and B high and 0.0f for the other pins -TEST(BrushedControlLoopTests, compute_motor_speed_outputs_full_reverse_speed) { - BrushedControlLoopTest brushed_control_loop; +// Test the run state function with a state of RUN and a speed of -0.3. +// Expect the A duty cycle to be 0.0, and the B duty cycle to be 0.3 +// Expect A low side gpio to be true, and B low side gpio to be false +TEST(BrushedControlLoopTest, test_run_negative_dir) { + BrushedControlLoopTest control_loop; - BrushedControlLoop::BrushedControlLoopParams params{ - .brake_mode_enabled = false, - .h_bridge_dead_time_us = 0, + control_loop.init(&control_loop.default_params); + control_loop.state_ = BrushedControlLoop::BrushedControlLoopState::RUN; + + hwbridge::HBridge::HBridgeInput expected_output{ + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = true, + .duty_cycle_b_h = 0.3f, + .low_side_b_gpio_state = false, }; - brushed_control_loop.init(¶ms); + // Get the actual output + hwbridge::HBridge::HBridgeInput actual_output = control_loop.run_state(-0.3f, control_loop.state_); - BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( - -control_loop::ControlLoop::MAX_MOTOR_SPEED, false, params.h_bridge_dead_time_us); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 1.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 1.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 0.0f); + // Compare the actual output to the expected output + EXPECT_EQ(actual_output.duty_cycle_a_h, expected_output.duty_cycle_a_h); + EXPECT_EQ(actual_output.low_side_a_gpio_state, expected_output.low_side_a_gpio_state); + EXPECT_EQ(actual_output.duty_cycle_b_h, expected_output.duty_cycle_b_h); + EXPECT_EQ(actual_output.low_side_b_gpio_state, expected_output.low_side_b_gpio_state); } -// Test that a motor speed of 0.5f returns a duty cycle of 0.5f A high and B low and 0.0f for the other pins -TEST(BrushedControlLoopTests, compute_motor_speed_outputs_half_speed) { - BrushedControlLoopTest brushed_control_loop; +// Do an E2E test of the control loop where we run the control loop at a speed of 0.3 +// Expect the mock bridge to be called with the appropriate duty cycles and gpio states +// Then, run the control loop at a speed of -0.6 +// Expect a dead time pause to occur +// After dead time, expect the mock bridge to be called with the appropriate duty cycles and gpio states +// Then, run the control loop at a speed of 0.0 +// Expect the mock bridge to be called with the appropriate duty cycles and gpio states of the brake mode +TEST(BrushedControlLoopTest, test_pos_to_neg_then_stop) { + BrushedControlLoopTest control_loop; + BrushedControlLoop::BrushedControlLoopParams params{.brake_mode = BrushedControlLoop::BrushedBrakeType::BRAKE_LOW_SIDE, + .deadtime_us = 60, + .current_controller_params = { + .kp = 0.0f, + .ki = 0.0f, + .kd = 0.0f, + }}; + + control_loop.init(¶ms); + // Set the mock time to 1 + EXPECT_CALL(control_loop.mock_clock_, get_time_us()).WillOnce(Return(1)); + + // Set the mock bridge expectations + hwbridge::HBridge::HBridgeInput expected_output = { + .duty_cycle_a_h = 0.3f, + .low_side_a_gpio_state = false, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = true, + }; + EXPECT_CALL(control_loop.mock_bridge_, run(expected_output)).WillOnce(Return(app_hal_status_E::APP_HAL_OK)); - BrushedControlLoop::BrushedControlLoopParams params{ - .brake_mode_enabled = false, - .h_bridge_dead_time_us = 0, + // Run the control loop + control_loop.run(0.3f); + + // Set the mock time to 20 + EXPECT_CALL(control_loop.mock_clock_, get_time_us()).WillOnce(Return(20)); + + // Set the mock bridge expectations + expected_output = { + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = false, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = false, }; - brushed_control_loop.init(¶ms); + EXPECT_CALL(control_loop.mock_bridge_, run(expected_output)).WillOnce(Return(app_hal_status_E::APP_HAL_OK)); - BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( - control_loop::ControlLoop::MAX_MOTOR_SPEED * 0.5f, false, params.h_bridge_dead_time_us); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.5f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 0.5f); -} + // Run the control loop + control_loop.run(-0.6f); -// Test the dead time by commanding -0.5f and then 0.5f and checking that the duty cycle is 0.0f for pins -TEST(BrushedControlLoopTests, compute_motor_speed_outputs_dead_time) { - utime_t test_time = 100000; - BrushedControlLoopTest brushed_control_loop; + // Set the mock bridge expectations to be floating until 20 + params.deadtime_us. In this case, set the mock time to that + // value - 1 + EXPECT_CALL(control_loop.mock_clock_, get_time_us()).WillOnce(Return(20 + params.deadtime_us - 1)); - BrushedControlLoop::BrushedControlLoopParams params{ - .brake_mode_enabled = false, - .h_bridge_dead_time_us = 10, + // Set the mock bridge expectations + expected_output = { + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = false, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = false, + }; + EXPECT_CALL(control_loop.mock_bridge_, run(expected_output)).WillOnce(Return(app_hal_status_E::APP_HAL_OK)); + + // Run the control loop + control_loop.run(-0.6f); + + // Now, set the mock time to 20 + params.deadtime_us + EXPECT_CALL(control_loop.mock_clock_, get_time_us()).WillOnce(Return(20 + params.deadtime_us)); + + // Set the mock bridge expectations + expected_output = { + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = true, + .duty_cycle_b_h = 0.6f, + .low_side_b_gpio_state = false, + }; + + EXPECT_CALL(control_loop.mock_bridge_, run(expected_output)).WillOnce(Return(app_hal_status_E::APP_HAL_OK)); + + // Run the control loop + control_loop.run(-expected_output.duty_cycle_b_h); + + // Set the mock time to 1000 + EXPECT_CALL(control_loop.mock_clock_, get_time_us()).WillOnce(Return(1000)); + + // Set the mock bridge expectations + expected_output = { + .duty_cycle_a_h = 0.0f, + .low_side_a_gpio_state = true, + .duty_cycle_b_h = 0.0f, + .low_side_b_gpio_state = true, }; + EXPECT_CALL(control_loop.mock_bridge_, run(expected_output)).WillOnce(Return(app_hal_status_E::APP_HAL_OK)); - brushed_control_loop.init(¶ms); - - BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = - brushed_control_loop.compute_motor_speed_outputs(-0.5f * control_loop::ControlLoop::MAX_MOTOR_SPEED, false, test_time); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 0.5f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.5f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 0.0f); - motor_speed_outputs = - brushed_control_loop.compute_motor_speed_outputs(0.5f * control_loop::ControlLoop::MAX_MOTOR_SPEED, false, test_time + 1); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 0.0f); - motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs(control_loop::ControlLoop::MAX_MOTOR_SPEED * 0.5f, - false, params.h_bridge_dead_time_us + test_time + 1); - EXPECT_EQ(motor_speed_outputs.DC_A_HIGH, 0.5f); - EXPECT_EQ(motor_speed_outputs.DC_A_LOW, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_HIGH, 0.0f); - EXPECT_EQ(motor_speed_outputs.DC_B_LOW, 0.5f); + // Run the control loop + control_loop.run(0.0f); } } // namespace control_loop diff --git a/test/mocks/mock_hbridge.hpp b/test/mocks/mock_hbridge.hpp new file mode 100644 index 0000000..830a3ee --- /dev/null +++ b/test/mocks/mock_hbridge.hpp @@ -0,0 +1,18 @@ +#ifndef MOCK_H_BRIDGE_HPP +#define MOCK_H_BRIDGE_HPP + +#include "bridge_hbridge.hpp" +#include "gmock/gmock.h" + +namespace hwbridge { +// Define a mock class for an HBridge +class MOCK_HBRIDGE : public HBridge { + public: + MOCK_METHOD(app_hal_status_E, run, (HBridgeInput input), (override)); + MOCK_METHOD(app_hal_status_E, get_current, (float& current), (override)); + MOCK_METHOD(app_hal_status_E, get_voltage, (float& voltage), (override)); +}; + +} // namespace hwbridge + +#endif // MOCK_H_BRIDGE_HPP \ No newline at end of file diff --git a/test/stepper_motor_test.cpp b/test/stepper_motor_test.cpp index aeb5517..ed33cdd 100644 --- a/test/stepper_motor_test.cpp +++ b/test/stepper_motor_test.cpp @@ -1,103 +1,54 @@ #include "bridge_hbridge_drv8801.hpp" #include "gmock/gmock.h" #include "gtest/gtest.h" +#include "math_util.hpp" #include "mock_hal_clock.hpp" +#include "mock_hbridge.hpp" #include "stepper_control_loop.hpp" +// Make a class that inherits from StepperControlLoop so we can test it namespace control_loop { -// Test the determineElectricalAngle function. For now, this will use simple 4 block commutation +class StepperControlLoopTest : public control_loop::StepperControlLoop { + public: + StepperControlLoopParams default_params{.i_hold = 0.5, .i_run = 1.0, .max_speed = 10.0}; -// Assert that a desired speed of 0 returns the same previous angle -TEST(StepperControlLoop, determineElectricalAngleZeroSpeed) { - // Create a mock clock + hwbridge::MOCK_HBRIDGE bridge_a; + hwbridge::MOCK_HBRIDGE bridge_b; basilisk_hal::MOCK_HAL_CLOCK clock; - // Create a mock motor - hwbridge::HBridgeDRV8801 motor; - // Create a stepper control loop - StepperControlLoop stepper_control_loop(motor, motor, clock); - // Assert that the angle is the same as the previous angle - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(0.0f, 0.0f, 0.0f), 0.0f); - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(0.0f, 0.0f, 90.0f), 90.0f); - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(0.0f, 0.0f, 180.0f), 180.0f); - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(0.0f, 0.0f, 270.0f), 270.0f); - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(0.0f, 0.0f, 360.0f), 360.0f); -} - -// Assert that at a desired speed's time delta, the angle is 90 degrees greater than previous angle -TEST(StepperControlLoop, determineElectricalAngleMaxSpeed) { - // Create a mock clock - basilisk_hal::MOCK_HAL_CLOCK clock; - // Create a mock motor - hwbridge::HBridgeDRV8801 motor; - // Create a stepper control loop - StepperControlLoop stepper_control_loop(motor, motor, clock); - const float test_max_speed_steps_per_second = 200.0f; - // Determine the time delta to be at max speed - const float test_time_delta = 1.0f / test_max_speed_steps_per_second; - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(test_time_delta, test_max_speed_steps_per_second, 0.0f), 90.0f); - // make an angle 1 degree. Expect the angle to be 271 degrees - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(test_time_delta, -test_max_speed_steps_per_second, 1.0f), 271.0f); - // make an angle 359 degrees. Expect the angle to be 89 degrees - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(test_time_delta, test_max_speed_steps_per_second, 359.0f), 89.0f); -} - -// Microstepping test: assert that at half a desired's speed's time delta, the angle is 45 degrees greater than previous angle -TEST(StepperControlLoop, determineElectricalAngleHalfSpeed) { - // Create a mock clock - basilisk_hal::MOCK_HAL_CLOCK clock; - // Create a mock motor - hwbridge::HBridgeDRV8801 motor; - // Create a stepper control loop - StepperControlLoop stepper_control_loop(motor, motor, clock); - const float test_max_speed_steps_per_second = 200.0f; - // Determine the time delta to be at max speed - const float test_time_delta = 1.0f / (2.0f * test_max_speed_steps_per_second); - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(test_time_delta, test_max_speed_steps_per_second, 0.0f), 45.0f); - // make an angle 1 degree. Expect the angle to be 316 degrees - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(test_time_delta, -test_max_speed_steps_per_second, 1.0f), 316.0f); - // make an angle 359 degrees. Expect the angle to be 44 degrees - EXPECT_EQ(stepper_control_loop.determineElectricalAngle(test_time_delta, test_max_speed_steps_per_second, 359.0f), 44.0f); -} -// Test the motor current value calculation function -// Assert that a desired angle of 0 makes the A current scaler 1 and B current scaler 0 -TEST(StepperControlLoop, determineMotorCurrentValuesAngle0) { - // Create a mock clock - basilisk_hal::MOCK_HAL_CLOCK clock; - // Create a mock motor - hwbridge::HBridgeDRV8801 motor; - // Create a stepper control loop - StepperControlLoop stepper_control_loop(motor, motor, clock); - // Assert that the angle is the same as the previous angle - EXPECT_EQ(stepper_control_loop.determineCurrentSetpointScalars(0.0f), std::make_pair(1.0f, 0.0f)); -} - -// Assert that a desired angle of 90 makes the A current scaler 0 and B current scaler 1 -TEST(StepperControlLoop, determineMotorCurrentValuesAngle90) { - // Create a mock clock - basilisk_hal::MOCK_HAL_CLOCK clock; - // Create a mock motor - hwbridge::HBridgeDRV8801 motor; - // Create a stepper control loop - StepperControlLoop stepper_control_loop(motor, motor, clock); - // Assert that the angle is the same as the previous angle - std::pair current_scalars = stepper_control_loop.determineCurrentSetpointScalars(90.0f); - EXPECT_NEAR(current_scalars.first, 0.0f, 0.01); - EXPECT_NEAR(current_scalars.second, 1.0f, 0.01); -} - -// Assert that a desired angle of 315 makes the A current scaler 1/sqrt(2) and B current scaler -1/sqrt(2) -TEST(StepperControlLoop, determineMotorCurrentValuesAngle315) { - // Create a mock clock - basilisk_hal::MOCK_HAL_CLOCK clock; - // Create a mock motor - hwbridge::HBridgeDRV8801 motor; - // Create a stepper control loop - StepperControlLoop stepper_control_loop(motor, motor, clock); - // Assert that the angle is the same as the previous angle - std::pair current_scalars = stepper_control_loop.determineCurrentSetpointScalars(315.0f); - EXPECT_NEAR(current_scalars.first, 1.0f / std::sqrt(2.0f), 0.01); - EXPECT_NEAR(current_scalars.second, -1.0f / std::sqrt(2.0f), 0.01); + BrushedControlLoop brushed_control_loop_a{bridge_a, clock}; + BrushedControlLoop brushed_control_loop_b{bridge_b, clock}; + + StepperControlLoopTest() : StepperControlLoop(brushed_control_loop_a, brushed_control_loop_b, clock) {} + // Make the private functions public so we can test them + using StepperControlLoop::determine_current_setpoints; + + // Make the protected variables public so we can test them + using StepperControlLoop::electrical_angle_; +}; + +TEST(stepper_motor_test, test_current_setpoint) { + StepperControlLoopTest stepper_control_loop_test; + // Test the determine_current_setpoint_scalars function + // Test the case where the electrical angle is 0 + auto current_setpoint_scalars = stepper_control_loop_test.determine_current_setpoints(1.0, 0); + EXPECT_NEAR(current_setpoint_scalars.first, 1.0, math::ACCEPTABLE_FLOAT_ERROR); + EXPECT_NEAR(current_setpoint_scalars.second, 0.0, math::ACCEPTABLE_FLOAT_ERROR); + + // Test the case where the electrical angle is pi/2 + current_setpoint_scalars = stepper_control_loop_test.determine_current_setpoints(1.0, math::M_PI_FLOAT / 2); + EXPECT_NEAR(current_setpoint_scalars.first, 0.0, math::ACCEPTABLE_FLOAT_ERROR); + EXPECT_NEAR(current_setpoint_scalars.second, 1.0, math::ACCEPTABLE_FLOAT_ERROR); + + // Test the case where the electrical angle is pi + current_setpoint_scalars = stepper_control_loop_test.determine_current_setpoints(1.0, math::M_PI_FLOAT); + EXPECT_NEAR(current_setpoint_scalars.first, -1.0, math::ACCEPTABLE_FLOAT_ERROR); + EXPECT_NEAR(current_setpoint_scalars.second, 0.0, math::ACCEPTABLE_FLOAT_ERROR); + + // Test the case where the electrical angle is 3pi/2 + current_setpoint_scalars = stepper_control_loop_test.determine_current_setpoints(1.0, 3 * math::M_PI_FLOAT / 2); + EXPECT_NEAR(current_setpoint_scalars.first, 0.0, math::ACCEPTABLE_FLOAT_ERROR); + EXPECT_NEAR(current_setpoint_scalars.second, -1.0, math::ACCEPTABLE_FLOAT_ERROR); } } // namespace control_loop \ No newline at end of file diff --git a/util/math/math_util.hpp b/util/math/math_util.hpp index 75bef56..025799d 100644 --- a/util/math/math_util.hpp +++ b/util/math/math_util.hpp @@ -1,5 +1,6 @@ #ifndef MATH_UTIL_HPP #define MATH_UTIL_HPP +#include #ifndef M_PI #define M_PI 3.14159265358979323846 @@ -12,6 +13,8 @@ constexpr float sqrt_3_over_3 = 0.5773502691896258; constexpr float M_PI_FLOAT = static_cast(M_PI); +constexpr float ACCEPTABLE_FLOAT_ERROR = 0.000001; + /** * @brief Integrate the given value using the trapezoidal rule. * @param x The current x value @@ -64,6 +67,29 @@ void wraparound(T& value, const T& min, const T& max) { * @param dt The time step */ float low_pass_filter(float input, float prev_output, float tau, float dt); + +// Make a function to get the sign of a number of any type +/** + * @brief Return the sign of a number. + * @param val The number to get the sign of + * @return int The sign of the number + */ +template +int8_t sign(T val) { + return (T(0) < val) - (val < T(0)); +} + +// Return the maximum of two values +/** + * @brief Return the maximum of two values. + * @param a The first value + * @param b The second value + */ +template +T max(const T& a, const T& b) { + return (a > b) ? a : b; +} + } // namespace math #endif // MATH_UTIL_HPP diff --git a/util/util.hpp b/util/util.hpp new file mode 100644 index 0000000..0024f69 --- /dev/null +++ b/util/util.hpp @@ -0,0 +1,7 @@ +#ifndef UTIL_HPP +#define UTIL_HPP + +// Define an unused macro to silence compiler warnings +#define UNUSED(x) (void)(x) + +#endif // UTIL_HPP \ No newline at end of file