From 09caa513ac875d3f15bfc24c7e513d725157746b Mon Sep 17 00:00:00 2001 From: Sahil Kale Date: Mon, 9 Oct 2023 11:43:34 +0200 Subject: [PATCH] [Initial Commit] Port main logic from other repo Topic: inital-commit Relative: Reviewers: --- .clang-format | 4 + .github/workflows/format.yml | 13 + .github/workflows/test.yml | 13 + .gitignore | 2 + .gitmodules | 3 + CMakeLists.txt | 34 +++ inc/bridge_3phase.hpp | 67 ++++ inc/bridge_3phase_drv8323.hpp | 164 ++++++++++ inc/bridge_hbridge.hpp | 18 ++ inc/bridge_hbridge_drv8801.hpp | 59 ++++ inc/brushed_control_loop.hpp | 56 ++++ inc/brushless_6step_control_loop.hpp | 140 +++++++++ inc/brushless_foc_control_loop.hpp | 249 +++++++++++++++ inc/comm_manager.hpp | 99 ++++++ inc/comm_message_types.hpp | 123 ++++++++ inc/control_loop.hpp | 17 ++ inc/example.hpp | 7 + inc/hal_adc.hpp | 22 ++ inc/hal_clock.hpp | 26 ++ inc/hal_common.hpp | 21 ++ inc/hal_gpio.hpp | 50 +++ inc/hal_serial.hpp | 40 +++ inc/hal_timer.hpp | 75 +++++ inc/input_manager.hpp | 50 +++ inc/math_foc.hpp | 144 +++++++++ inc/param_service.hpp | 92 ++++++ inc/pid.hpp | 78 +++++ inc/stepper_control_loop.hpp | 56 ++++ inc/system_manager.hpp | 98 ++++++ libs/googletest | 1 + scripts/format.sh | 17 ++ scripts/gdb.sh | 1 + scripts/setup.sh | 13 + scripts/test.sh | 29 ++ scripts/test_clang_format.sh | 30 ++ src/brushed_control_loop.cpp | 95 ++++++ src/brushless_6step_control_loop.cpp | 274 +++++++++++++++++ src/brushless_foc_control_loop.cpp | 338 +++++++++++++++++++++ src/comm_manager.cpp | 119 ++++++++ src/example.cpp | 5 + src/input_manager.cpp | 55 ++++ src/math_foc.cpp | 117 +++++++ src/param_service.cpp | 7 + src/pid.cpp | 109 +++++++ src/stepper_control_loop.cpp | 86 ++++++ src/system_manager.cpp | 134 ++++++++ test/CMakeLists.txt | 23 ++ test/brushed_control_loop_test.cpp | 117 +++++++ test/brushless_6step_control_loop_test.cpp | 224 ++++++++++++++ test/brushless_foc_control_loop_test.cpp | 187 ++++++++++++ test/comm_manager_test.cpp | 141 +++++++++ test/foc_math_test.cpp | 120 ++++++++ test/input_manager_test.cpp | 67 ++++ test/main.cpp | 6 + test/mock_bridge_3phase.hpp | 27 ++ test/mock_hal_adc.hpp | 13 + test/mock_hal_clock.hpp | 15 + test/mock_hal_serial.hpp | 22 ++ test/mock_hal_timer.hpp | 38 +++ test/param_test.cpp | 7 + test/pid_test.cpp | 73 +++++ test/stepper_motor_test.cpp | 103 +++++++ test/system_manager_test.cpp | 91 ++++++ test/test.cpp | 4 + 64 files changed, 4528 insertions(+) create mode 100644 .clang-format create mode 100644 .github/workflows/format.yml create mode 100644 .github/workflows/test.yml create mode 100644 .gitmodules create mode 100644 CMakeLists.txt create mode 100644 inc/bridge_3phase.hpp create mode 100644 inc/bridge_3phase_drv8323.hpp create mode 100644 inc/bridge_hbridge.hpp create mode 100644 inc/bridge_hbridge_drv8801.hpp create mode 100644 inc/brushed_control_loop.hpp create mode 100644 inc/brushless_6step_control_loop.hpp create mode 100644 inc/brushless_foc_control_loop.hpp create mode 100644 inc/comm_manager.hpp create mode 100644 inc/comm_message_types.hpp create mode 100644 inc/control_loop.hpp create mode 100644 inc/example.hpp create mode 100644 inc/hal_adc.hpp create mode 100644 inc/hal_clock.hpp create mode 100644 inc/hal_common.hpp create mode 100644 inc/hal_gpio.hpp create mode 100644 inc/hal_serial.hpp create mode 100644 inc/hal_timer.hpp create mode 100644 inc/input_manager.hpp create mode 100644 inc/math_foc.hpp create mode 100644 inc/param_service.hpp create mode 100644 inc/pid.hpp create mode 100644 inc/stepper_control_loop.hpp create mode 100644 inc/system_manager.hpp create mode 160000 libs/googletest create mode 100644 scripts/format.sh create mode 100644 scripts/gdb.sh create mode 100644 scripts/setup.sh create mode 100644 scripts/test.sh create mode 100644 scripts/test_clang_format.sh create mode 100644 src/brushed_control_loop.cpp create mode 100644 src/brushless_6step_control_loop.cpp create mode 100644 src/brushless_foc_control_loop.cpp create mode 100644 src/comm_manager.cpp create mode 100644 src/example.cpp create mode 100644 src/input_manager.cpp create mode 100644 src/math_foc.cpp create mode 100644 src/param_service.cpp create mode 100644 src/pid.cpp create mode 100644 src/stepper_control_loop.cpp create mode 100644 src/system_manager.cpp create mode 100644 test/CMakeLists.txt create mode 100644 test/brushed_control_loop_test.cpp create mode 100644 test/brushless_6step_control_loop_test.cpp create mode 100644 test/brushless_foc_control_loop_test.cpp create mode 100644 test/comm_manager_test.cpp create mode 100644 test/foc_math_test.cpp create mode 100644 test/input_manager_test.cpp create mode 100644 test/main.cpp create mode 100644 test/mock_bridge_3phase.hpp create mode 100644 test/mock_hal_adc.hpp create mode 100644 test/mock_hal_clock.hpp create mode 100644 test/mock_hal_serial.hpp create mode 100644 test/mock_hal_timer.hpp create mode 100644 test/param_test.cpp create mode 100644 test/pid_test.cpp create mode 100644 test/stepper_motor_test.cpp create mode 100644 test/system_manager_test.cpp create mode 100644 test/test.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..cf9e22b --- /dev/null +++ b/.clang-format @@ -0,0 +1,4 @@ +--- +BasedOnStyle: Google +IndentWidth: 4 +ColumnLimit: 130 \ No newline at end of file diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000..cca255f --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,13 @@ +name: Code Format check + +on: + pull_request: + branches: [ main ] + +jobs: + check-format: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - run: sudo apt install clang-format + - run: bash scripts/test_clang_format.sh \ No newline at end of file diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml new file mode 100644 index 0000000..37b1d73 --- /dev/null +++ b/.github/workflows/test.yml @@ -0,0 +1,13 @@ +name: Test + +on: + pull_request: + branches: [ main ] + +jobs: + test: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - run: bash scripts/setup.sh + - run: bash scripts/test.sh \ No newline at end of file diff --git a/.gitignore b/.gitignore index 259148f..ee6d40f 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,5 @@ *.exe *.out *.app + +build/ \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..26b7b90 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "libs/googletest"] + path = libs/googletest + url = https://github.com/google/googletest.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..82a5b38 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.8) + +set(This MAINS) +set(BINARY ${CMAKE_PROJECT_NAME}) + +project(${This} C CXX) + +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +set(BUILD_GMOCK ON) + +# enable Werror +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror") + +enable_testing() + +# add the googletest subdirectory, located in the lib folder tree that is one level above this one +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/libs/googletest ${CMAKE_CURRENT_BINARY_DIR}/googletest) + +# set the HEADERS as everything in the inc folder, as well as test *.hpp files +file(GLOB HEADERS "inc/*.hpp") + +# set the SOURCES as everything in the src folder +file(GLOB SOURCES "src/*.cpp") + +# add_executable(${BINARY}_run ${SOURCES}) +add_library(${This} STATIC ${SOURCES} ${HEADERS}) + +add_subdirectory(test) + +# To find the tests, this is rquired +include_directories(inc) +include_directories(src) \ No newline at end of file diff --git a/inc/bridge_3phase.hpp b/inc/bridge_3phase.hpp new file mode 100644 index 0000000..cc60c05 --- /dev/null +++ b/inc/bridge_3phase.hpp @@ -0,0 +1,67 @@ +#ifndef BRIDGE_3PHASE_HPP +#define BRIDGE_3PHASE_HPP + +#include "hal_common.hpp" + +namespace hwbridge { + +// Define a generic 3-phase bridge class + +class Bridge3Phase { + public: + Bridge3Phase() = default; + virtual ~Bridge3Phase() = default; + + typedef struct phase_command { + float duty_cycle_high_side; + bool invert_low_side; + } phase_command_t; + + // Define a struct to return the PHASE (not backemf) voltage + typedef struct bemf_voltage { + float u; + float v; + float w; + } bemf_voltage_t; + + // Define a struct to return the current + typedef struct current { + float u; + float v; + float w; + } phase_current_t; + + virtual app_hal_status_E init() = 0; + + // Define a virtual function to set the individual phases' duty cycles and enable/disable the phase + virtual void set_phase(const phase_command_t& u, const phase_command_t& v, const phase_command_t& w) = 0; + + // Define a virtual function to get the back emf voltage + virtual app_hal_status_E read_bemf(bemf_voltage_t& bemf_voltage) = 0; + + // Define a virtual function to get the current + virtual app_hal_status_E read_current(phase_current_t& current) = 0; + + // Define a virtual function to get the bus voltage + virtual app_hal_status_E read_bus_voltage(float& bus_voltage) = 0; + + static constexpr uint8_t NUM_PHASES = 3; +}; + +// Define a generic class for a sensor that returns the sector of the rotor +class BldcRotorSectorSensor { + public: + BldcRotorSectorSensor() = default; + virtual ~BldcRotorSectorSensor() = default; + + // Define a virtual function to initialize the sensor + virtual app_hal_status_E init() = 0; + + // Define a virtual function to get the sector + // @RETURNS the sector (0-5) + virtual app_hal_status_E get_sector(uint8_t& sector) = 0; +}; + +} // namespace hwbridge + +#endif // BRIDGE_3PHASE_HPP \ No newline at end of file diff --git a/inc/bridge_3phase_drv8323.hpp b/inc/bridge_3phase_drv8323.hpp new file mode 100644 index 0000000..dc8d12d --- /dev/null +++ b/inc/bridge_3phase_drv8323.hpp @@ -0,0 +1,164 @@ +#ifndef BRIDGE_3PHASE_DRV8323_HPP +#define BRIDGE_3PHASE_DRV8323_HPP + +#include + +#include "bridge_3phase.hpp" +#include "control_loop.hpp" +#include "hal_adc.hpp" +#include "hal_common.hpp" +#include "hal_gpio.hpp" +#include "hal_timer.hpp" + +namespace hwbridge { + +// Define a 3-phase bridge class using DRV8323 +class Bridge3PhaseDRV8323 : public Bridge3Phase { + public: + typedef struct drv8323_phase_config_info { + // Timer channel + // TODO: there should be a timer object here as well. Assuming all channels are on the same timer + basilisk_hal::timer_channel_E channel; + basilisk_hal::HAL_GPIO* low_side_gpio; // Not supporting low side gate control for now except for enable/disable + basilisk_hal::HAL_ADC* bemf_sense_adc; + uint8_t bemf_sense_adc_channel; + // TODO: add current sense support. For now, only 6 step is supported. + // basilisk_hal::HAL_ADC* current_sense_adc; + // uint8_t current_sense_adc_channel; + } drv8323_phase_config_info_t; + + Bridge3PhaseDRV8323(basilisk_hal::HAL_ComplementaryPWM_Timer& timer, drv8323_phase_config_info_t& u, + drv8323_phase_config_info_t& v, drv8323_phase_config_info_t& w, uint32_t frequency) { + // Register the timer and pin + pwm_timer_ = &timer; + u_ = u; + v_ = v; + w_ = w; + frequency_ = frequency; + }; + ~Bridge3PhaseDRV8323() = default; + + app_hal_status_E init() override { + // Set the direction pin to output + u_.low_side_gpio->set_mode(basilisk_hal::gpio_mode_E::OUTPUT); + v_.low_side_gpio->set_mode(basilisk_hal::gpio_mode_E::OUTPUT); + w_.low_side_gpio->set_mode(basilisk_hal::gpio_mode_E::OUTPUT); + + // Set the pwm timer to PWM_CENTRE_ALIGNED + pwm_timer_->set_timer_params(basilisk_hal::timer_mode_E::PWM_CENTRE_ALIGNED, frequency_); + + // Set the timer channel + pwm_timer_->set_channel(0, u_.channel); + pwm_timer_->set_channel(0, v_.channel); + pwm_timer_->set_channel(0, w_.channel); + + // Start the timer + pwm_timer_->start(); + + // Start PWM + pwm_timer_->start_pwm(u_.channel); + pwm_timer_->start_pwm(v_.channel); + pwm_timer_->start_pwm(w_.channel); + + return app_hal_status_E::APP_HAL_OK; + } + + // Define a virtual function to set the individual phases' duty cycles and enable/disable the phase + void set_phase(const phase_command_t& u, const phase_command_t& v, const phase_command_t& w) { + // Determine the appropriate amount of ticks by getting the timer period + uint32_t period = pwm_timer_->get_period(); + uint32_t ticks_u = + static_cast(period * fabs(u.duty_cycle_high_side) / control_loop::ControlLoop::MAX_MOTOR_SPEED); + uint32_t ticks_v = + static_cast(period * fabs(v.duty_cycle_high_side) / control_loop::ControlLoop::MAX_MOTOR_SPEED); + uint32_t ticks_w = + static_cast(period * fabs(w.duty_cycle_high_side) / control_loop::ControlLoop::MAX_MOTOR_SPEED); + + // Determine the low-side phase active high or active low signal for the complementary timer + // If the inverted boolean is set in the phase command, we should set the command + // to active low, otherwise, we should set the command to active high + + basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E u_complementary_phase = + u.invert_low_side ? basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E::ACTIVE_LOW + : basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E::ACTIVE_HIGH; + + basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E v_complementary_phase = + v.invert_low_side ? basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E::ACTIVE_LOW + : basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E::ACTIVE_HIGH; + + basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E w_complementary_phase = + w.invert_low_side ? basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E::ACTIVE_LOW + : basilisk_hal::HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E::ACTIVE_HIGH; + + // Set the PWM timer + pwm_timer_->set_channel_with_complementary_phase(ticks_u, u_.channel, u_complementary_phase); + pwm_timer_->set_channel_with_complementary_phase(ticks_v, v_.channel, v_complementary_phase); + pwm_timer_->set_channel_with_complementary_phase(ticks_w, w_.channel, w_complementary_phase); + } + + // Define a function to read the BEMF voltage + app_hal_status_E read_bemf(bemf_voltage_t& bemf_voltage) { + app_hal_status_E status = app_hal_status_E::APP_HAL_OK; + if (u_.bemf_sense_adc == nullptr || v_.bemf_sense_adc == nullptr || w_.bemf_sense_adc == nullptr) { + status = app_hal_status_E::APP_HAL_NOT_INITIALIZED; + } else { + do { + status = u_.bemf_sense_adc->read_adc(bemf_voltage.u, u_.bemf_sense_adc_channel); + if (status != app_hal_status_E::APP_HAL_OK) { + break; + } + status = v_.bemf_sense_adc->read_adc(bemf_voltage.v, v_.bemf_sense_adc_channel); + if (status != app_hal_status_E::APP_HAL_OK) { + break; + } + status = w_.bemf_sense_adc->read_adc(bemf_voltage.w, w_.bemf_sense_adc_channel); + if (status != app_hal_status_E::APP_HAL_OK) { + break; + } + } while (0); + } + + return status; + } + + // Define a function to read the current + app_hal_status_E read_current(phase_current_t& current) { + IGNORE(current); + return app_hal_status_E::APP_HAL_NOT_IMPLEMENTED; + } + + // Define a function that averages out the BEMF voltage readings. Returns a bemf_voltage_t object that contains the + // average of the BEMF voltage readings. + bemf_voltage_t average_bemf_readings(const bemf_voltage_t& new_reading) { + bemf_voltage_t average_bemf_voltage = {0, 0, 0}; + for (uint8_t i = 0; i < BEMF_VOLTAGE_AVERAGE_SIZE - 1; i++) { + bemf_voltage_[i] = bemf_voltage_[i + 1]; + average_bemf_voltage.u += bemf_voltage_[i].u; + average_bemf_voltage.v += bemf_voltage_[i].v; + average_bemf_voltage.w += bemf_voltage_[i].w; + } + bemf_voltage_[BEMF_VOLTAGE_AVERAGE_SIZE - 1] = new_reading; + average_bemf_voltage.u += new_reading.u; + average_bemf_voltage.v += new_reading.v; + average_bemf_voltage.w += new_reading.w; + average_bemf_voltage.u /= BEMF_VOLTAGE_AVERAGE_SIZE; + average_bemf_voltage.v /= BEMF_VOLTAGE_AVERAGE_SIZE; + average_bemf_voltage.w /= BEMF_VOLTAGE_AVERAGE_SIZE; + return average_bemf_voltage; + } + + private: + // Define a timer object + basilisk_hal::HAL_ComplementaryPWM_Timer* pwm_timer_; + drv8323_phase_config_info_t u_; + drv8323_phase_config_info_t v_; + drv8323_phase_config_info_t w_; + uint32_t frequency_; + static constexpr uint8_t BEMF_VOLTAGE_AVERAGE_SIZE = 10; + // Define an array of bemf voltage objects to average out the readings + bemf_voltage_t bemf_voltage_[BEMF_VOLTAGE_AVERAGE_SIZE]; +}; + +} // namespace hwbridge + +#endif // BRIDGE_3PHASE_DRV8323_HPP diff --git a/inc/bridge_hbridge.hpp b/inc/bridge_hbridge.hpp new file mode 100644 index 0000000..53463b0 --- /dev/null +++ b/inc/bridge_hbridge.hpp @@ -0,0 +1,18 @@ +#ifndef BRIDGE_HBRIDGE_HPP +#define BRIDGE_HBRIDGE_HPP + +namespace hwbridge { + +// Define a generic h-bridge class +class HBridge { + public: + HBridge() = default; + virtual ~HBridge() = default; + + // Define a virtual function to run the h-bridge + virtual void run(float speed) = 0; +}; + +} // namespace hwbridge + +#endif \ No newline at end of file diff --git a/inc/bridge_hbridge_drv8801.hpp b/inc/bridge_hbridge_drv8801.hpp new file mode 100644 index 0000000..26b061c --- /dev/null +++ b/inc/bridge_hbridge_drv8801.hpp @@ -0,0 +1,59 @@ +#ifndef BRIDGE_HBRIDGE_DRV8801_HPP +#define BRIDGE_HBRIDGE_DRV8801_HPP +#include + +#include "bridge_hbridge.hpp" +#include "control_loop.hpp" +#include "hal_gpio.hpp" +#include "hal_timer.hpp" + +namespace hwbridge { + +// Define a class that inherits from HBridge +class HBridgeDRV8801 : public HBridge { + public: + HBridgeDRV8801() = default; + ~HBridgeDRV8801() = default; + + void init(basilisk_hal::HAL_Timer& timer, basilisk_hal::timer_channel_E channel, basilisk_hal::HAL_GPIO& pin, + uint32_t frequency) { + // Register the timer and pin + pwm_timer_ = &timer; + dir_pin_ = &pin; + channel_ = channel; + + // Set the direction pin to output + dir_pin_->set_mode(basilisk_hal::gpio_mode_E::OUTPUT); + + // Set the pwm timer to PWM_CENTRE_ALIGNED + pwm_timer_->set_timer_params(basilisk_hal::timer_mode_E::PWM_CENTRE_ALIGNED, frequency); + + // Set the timer channel + pwm_timer_->set_channel(0, channel_); + + // Start the timer + pwm_timer_->start(); + } + + // 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); + + // Determine the appropriate amount of ticks by getting the timer period + uint32_t period = pwm_timer_->get_period(); + uint32_t ticks = static_cast(period * fabs(speed) / control_loop::ControlLoop::MAX_MOTOR_SPEED); + + // Set the PWM timer + pwm_timer_->set_channel(ticks, channel_); + } + + private: + // Define a timer object + basilisk_hal::HAL_Timer* pwm_timer_; + basilisk_hal::timer_channel_E channel_; + basilisk_hal::HAL_GPIO* dir_pin_; +}; + +} // namespace hwbridge +#endif // BRIDGE_HBRIDGE_DRV8801_HPP \ No newline at end of file diff --git a/inc/brushed_control_loop.hpp b/inc/brushed_control_loop.hpp new file mode 100644 index 0000000..864f67e --- /dev/null +++ b/inc/brushed_control_loop.hpp @@ -0,0 +1,56 @@ +#ifndef BRUSHED_CONTROL_LOOP_HPP +#define BRUSHED_CONTROL_LOOP_HPP +#include + +#include "control_loop.hpp" +#include "hal_clock.hpp" +#ifdef UNIT_TEST +// forward declare the BrushedControlLoopTest class +class BrushedControlLoopTest; +#endif + +namespace control_loop { + +// Define a class BrushedControlLoop that inherits from ControlLoop + +class BrushedControlLoop : public ControlLoop { + public: + BrushedControlLoop(); + + // Define a run method that overrides the run method in ControlLoop + void run(float speed) override; + + // Function to register a timer with the control loop + + // Function to register a HAL clock with the control loop + void register_clock(basilisk_hal::HAL_CLOCK* clock) { clock_ = clock; } + + // 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; + + 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 + */ + h_bridge_motor_speed_outputs_t compute_motor_speed_outputs(float motor_speed, bool brake_mode, utime_t current_time_us); + + utime_t last_speed_dir_change_time_us_{0}; + float last_motor_speed_{0.0f}; + basilisk_hal::HAL_CLOCK* clock_{nullptr}; + +#ifdef UNIT_TEST + friend class BrushedControlLoopTest; +#endif +}; +} // namespace control_loop + +#endif \ No newline at end of file diff --git a/inc/brushless_6step_control_loop.hpp b/inc/brushless_6step_control_loop.hpp new file mode 100644 index 0000000..4c7e1a3 --- /dev/null +++ b/inc/brushless_6step_control_loop.hpp @@ -0,0 +1,140 @@ +#ifndef BRUSHLESS_6STEP_CONTROL_LOOP_HPP +#define BRUSHLESS_6STEP_CONTROL_LOOP_HPP + +// forward declare when in unit test +#if defined(UNIT_TEST) || defined(DEBUG) +class Brushless6StepControlLoopTest; +#endif + +#include + +#include "bridge_3phase.hpp" +#include "control_loop.hpp" +#include "hal_clock.hpp" +namespace control_loop { + +// Define a brushless 6step control loop class that inherits from ControlLoop +class Brushless6StepControlLoop : public ControlLoop { + public: + enum class Brushless6StepControlLoopState { + STOP, + START, // Sensorless startup state + RUN, // Sensorless run state + }; + + enum class CommutationSignal { + HIGH, + LOW, + Z_RISING, + Z_FALLING, + }; + + typedef union { + CommutationSignal signals[hwbridge::Bridge3Phase::NUM_PHASES]; + struct { + CommutationSignal u; + CommutationSignal v; + CommutationSignal w; + }; + } commutation_step_t; + + static constexpr uint8_t num_commutation_steps = 6; + const commutation_step_t commutation_steps[num_commutation_steps] = { + {CommutationSignal::Z_FALLING, CommutationSignal::HIGH, CommutationSignal::LOW}, + {CommutationSignal::LOW, CommutationSignal::HIGH, CommutationSignal::Z_RISING}, + {CommutationSignal::LOW, CommutationSignal::Z_FALLING, CommutationSignal::HIGH}, + {CommutationSignal::Z_RISING, CommutationSignal::LOW, CommutationSignal::HIGH}, + {CommutationSignal::HIGH, CommutationSignal::LOW, CommutationSignal::Z_FALLING}, + {CommutationSignal::HIGH, CommutationSignal::Z_RISING, CommutationSignal::LOW}, + }; + + Brushless6StepControlLoop(hwbridge::Bridge3Phase& motor, basilisk_hal::HAL_CLOCK& clock, + hwbridge::BldcRotorSectorSensor* rotor_sensor = nullptr) + : motor_(motor), clock_(clock) { + rotor_sensor_ = rotor_sensor; + } + + void run(float speed) override; + + protected: + hwbridge::Bridge3Phase& motor_; + basilisk_hal::HAL_CLOCK& clock_; + hwbridge::BldcRotorSectorSensor* rotor_sensor_; + Brushless6StepControlLoopState state_ = Brushless6StepControlLoopState::STOP; + utime_t time_at_start_ = 0; + utime_t zero_crossing_time_ = 0; + utime_t last_commutation_step_switch_time_ = 0; + uint8_t commutation_step_ = 0; + + // Make an array of delta times for each commutation step to store experimentally + // measured delta times + static constexpr uint8_t num_commutation_step_delta_samples = 100; + utime_t commutation_step_delta_times_[num_commutation_step_delta_samples] = {0}; + // Scale factor that determines a 'safety factor' for the commutation step delta times + // If we expect a commutation step to take 1000us, but it takes 1000us * scale_factor, then we will consider it a zero + // crossing + float commutation_step_delta_time_scale_factor_ = 2.0f; + // Variable to store the average commutation step delta time + utime_t average_commutation_step_delta_time_ = 0; + + float motor_speed_ = 0.0f; + + /** + * @brief Get the desired state of the control loop + */ + Brushless6StepControlLoopState get_desired_state(utime_t current_time_us, utime_t time_at_start, float motor_speed); + + /** + * @brief Detect 0 crossing of the motor + */ + bool zero_crossing_detected(const hwbridge::Bridge3Phase::bemf_voltage_t& bemf_voltage, uint8_t commutation_step); + + /** + * @brief Generate the duty cycles for the 3 phases based on the commutation step + * @param phase_command array of phase commands to be filled in, u-v-w + * @param commutation_step the commutation step to generate the duty cycles for + * @param motor_speed the motor speed to generate the duty cycles for. 0 - 1.0f + */ + void generate_commutation_duty_cycles(hwbridge::Bridge3Phase::phase_command_t phase_command[3], uint8_t commutation_step, + float motor_speed); + + /** + * @brief Switch the commutation step + */ + void commutation_step_switch_sensorless(utime_t current_time_us); + + void update_average_commutation_step_delta_time(utime_t current_time_us) { + average_commutation_step_delta_time_ = + calculate_average_commutation_step_delta_time(current_time_us - last_commutation_step_switch_time_, + commutation_step_delta_times_, num_commutation_step_delta_samples); + last_commutation_step_switch_time_ = current_time_us; + } + + // Make a function that determines the average commutation step delta time + // based on the commutation_step_delta_times_ array + /** + * @brief Calculate the average commutation step delta time + * + * @param new_commutation_step_delta_time the new commutation step delta time to add to the array + * @param average_commutation_step_delta_time_array the array of commutation step delta times + * @param size the size of the array + * + * @return utime_t the average commutation step delta time. NOTE: 0 if the array is not full + */ + utime_t calculate_average_commutation_step_delta_time(utime_t new_commutation_step_delta_time, + utime_t* average_commutation_step_delta_time_array, size_t size); + + void reset_commutation_step_delta_times() { + for (size_t i = 0; i < num_commutation_step_delta_samples; i++) { + commutation_step_delta_times_[i] = 0; + } + average_commutation_step_delta_time_ = 0; + } + +#if defined(UNIT_TEST) || defined(DEBUG) + friend class Brushless6StepControlLoopTest; +#endif +}; +} // namespace control_loop + +#endif // BRUSHLESS_6STEP_CONTROL_LOOP_HPP diff --git a/inc/brushless_foc_control_loop.hpp b/inc/brushless_foc_control_loop.hpp new file mode 100644 index 0000000..34abce1 --- /dev/null +++ b/inc/brushless_foc_control_loop.hpp @@ -0,0 +1,249 @@ +#ifndef BRUSHLESS_FOC_CONTROL_LOOP_HPP +#define BRUSHLESS_FOC_CONTROL_LOOP_HPP + +// forward declare when in unit test +#if defined(UNIT_TEST) || defined(DEBUG) +class BrushlessFocControlLoopTest; +#endif + +#include + +#include "bridge_3phase.hpp" +#include "control_loop.hpp" +#include "hal_clock.hpp" +#include "math_foc.hpp" +#include "pid.hpp" + +namespace control_loop { + +class BldcElectricalRotorPositionEstimator { + public: + BldcElectricalRotorPositionEstimator() = default; + virtual ~BldcElectricalRotorPositionEstimator() = default; + + /** + * @brief Update the rotor position estimator + */ + virtual app_hal_status_E update(utime_t time) = 0; + + /** + * @brief Get the rotor position + * @param rotor_position The rotor position as an electrical angle (radians) + */ + virtual app_hal_status_E get_rotor_position(float& rotor_position) = 0; + + /** + * @brief Get the rotor velocity + * @param rotor_velocity The rotor velocity as an electrical angular velocity (radians/s) + */ + virtual app_hal_status_E get_rotor_velocity(float& rotor_velocity) = 0; + + /** + * @brief get whether the rotor position estimation is valid + * + * @return true if the rotor position estimation is valid + */ + virtual bool is_estimation_valid() = 0; + + virtual app_hal_status_E reset_estimation() = 0; +}; + +// Define a brushless foc control loop class that inherits from ControlLoop +class BrushlessFocControlLoop : public ControlLoop { + public: + enum class BrushlessFocControlLoopState { + NOT_INITIALIZED, + STOP, + RUN, + }; + + // Define a control loop type + enum class BrushlessFocControlLoopType { + OPEN_LOOP, + CLOSED_LOOP, + }; + + // Define a pwm control type (Sine or Space-Vector) + enum class BrushlessFocPwmControlType { + SINE, + SPACE_VECTOR, + }; + + // Define control-loop specific parameters + class BrushlessFocControLoopParams { + public: + float kp_q_current; + float ki_q_current; + float kd_q_current; + float kp_d_current; + float ki_d_current; + float kd_d_current; + utime_t foc_start_timeout_period_us; + + float speed_to_iq_gain; // Converts speed to iq reference + float i_d_reference; + + float open_loop_full_speed_theta_velocity; // rad/s + + BrushlessFocPwmControlType pwm_control_type; + }; + + BrushlessFocControlLoop(hwbridge::Bridge3Phase& motor, basilisk_hal::HAL_CLOCK& clock, + BldcElectricalRotorPositionEstimator& rotor_position_estimator) + : bridge_(motor), clock_(clock), rotor_position_estimator_(rotor_position_estimator) {} + + /** + * @brief Initialize the control loop + * @param params The control loop parameters + */ + void init(BrushlessFocControLoopParams* params); + + /** + * @brief Run the control loop + * @param speed The desired speed of the motor (note: this is multiplied by the speed_to_iq_gain) + * @note: speed is from -1 -> 1 + */ + void run(float speed) override; + + ~BrushlessFocControlLoop() = default; + + protected: + BrushlessFocControlLoopState state_ = BrushlessFocControlLoopState::NOT_INITIALIZED; + hwbridge::Bridge3Phase& bridge_; + basilisk_hal::HAL_CLOCK& clock_; + BldcElectricalRotorPositionEstimator& rotor_position_estimator_; + // Control loop parameters + BrushlessFocControLoopParams* params_ = nullptr; + + // Control loop state variables + utime_t time_at_start_ = 0; + utime_t last_run_time_ = 0; + float rotor_position_open_loop_start_ = 0.0f; + float motor_speed_, rotor_position_ = 0; + + BrushlessFocControlLoopType control_loop_type_ = BrushlessFocControlLoopType::OPEN_LOOP; + + // Create 2 PID controllers for the Q and D currents + pid::PID pid_q_current_{0.0, 0, 0, 0, 0, 0}; + pid::PID pid_d_current_{0.0, 0, 0, 0, 0, 0}; + float desired_rotor_angle_open_loop_ = 0.0f; + + // duty cycles + float duty_cycle_u_h_, duty_cycle_v_h_, duty_cycle_w_h_ = 0.0f; + + // FOC variables + float i_quadrature_, i_direct_, V_quadrature_, V_direct_, V_alpha_, V_beta_ = 0.0f; + + /** + * @brief Get the desired state of the control loop + */ + BrushlessFocControlLoopState get_desired_state(float motor_speed, const BrushlessFocControlLoopState current_state); + + /** + * @brief Get the desired control loop type + * @param is_estimator_valid Whether the rotor position estimator is valid + * @return The desired control loop type + */ + BrushlessFocControlLoopType get_desired_control_loop_type(bool is_estimator_valid); + + /** + * @brief Determine the duty cycles for the inverter using the FOC algorithm by doing inverse park and vector control algo + * (inverse clarke or foc) + * @param theta The rotor angle (radians) + * @param Vdirect The alpha component of the voltage vector + * @param Vquardature The beta component of the voltage vector + * @param bus_voltage The bus voltage + * @param phase_command_u The duty cycle for phase u + * @param phase_command_v The duty cycle for phase v + * @param phase_command_w The duty cycle for phase w + */ + void determine_inverter_duty_cycles(float theta, float Vdirect, float Vquadrature, float bus_voltage, + BrushlessFocPwmControlType pwm_control_type, + hwbridge::Bridge3Phase::phase_command_t& phase_command_u, + hwbridge::Bridge3Phase::phase_command_t& phase_command_v, + hwbridge::Bridge3Phase::phase_command_t& phase_command_w); + +#if defined(UNIT_TEST) || defined(DEBUG) + friend class BrushlessFocControlLoopTest; +#endif +}; + +class BldcElectricalRotorPositionEstimatorFromHall : public BldcElectricalRotorPositionEstimator { + public: + BldcElectricalRotorPositionEstimatorFromHall(basilisk_hal::HAL_CLOCK& clock, hwbridge::BldcRotorSectorSensor& sector_sensor) + : clock_(clock), sector_sensor_(sector_sensor) {} + + typedef struct BldcElectricalRotorPositionEstimatorFromHallParams { + uint16_t num_hall_updates_to_start; + float max_estimate_angle_overrun; // How much to allow the estimator to overrun the hall angle (radians) + } BldcElectricalRotorPositionEstimatorFromHallParams_t; + + /** + * @brief Initialize the rotor position estimator + * @param params The rotor position estimator parameters + * @return app_hal_status_E the status of the initialization + */ + app_hal_status_E init(BldcElectricalRotorPositionEstimatorFromHallParams_t* params); + + /** + * @brief Update the rotor position estimator + * @param time The current time + * @return app_hal_status_E the status of the update + */ + app_hal_status_E update(utime_t time) override; + + /** + * @brief Get the rotor position + * @param rotor_position The rotor position as an electrical angle (radians) + * @return app_hal_status_E the status of the operation + * @note: rotor_position is from 0 -> 2*pi + */ + app_hal_status_E get_rotor_position(float& rotor_position) override; + + /** + * @brief Get the rotor velocity + * @param rotor_velocity The rotor velocity as an electrical angular velocity (radians/s) + * @return app_hal_status_E the status of the operation + */ + app_hal_status_E get_rotor_velocity(float& rotor_velocity) override; + + /** + * @brief Get the raw hall angle + * @param raw_hall_angle The raw hall angle (radians) + * @return app_hal_status_E the status of the operation + * @note: raw_hall_angle is from 0 -> 2*pi + */ + app_hal_status_E get_raw_hall_angle(float& raw_hall_angle) { + raw_hall_angle = raw_hall_angle_; + return APP_HAL_OK; + } + + /** + * @brief get whether the rotor position estimation is valid + * @return true if the rotor position estimation is valid + * @note: the rotor position estimation is valid if the number of hall updates is greater than the number of hall updates to + * start + */ + bool is_estimation_valid() override; + + /** + * @brief reset the rotor position estimation + * @return app_hal_status_E the status of the operation + */ + app_hal_status_E reset_estimation() override; + + basilisk_hal::HAL_CLOCK& clock_; + hwbridge::BldcRotorSectorSensor& sector_sensor_; + float rotor_position_ = 0.0f; + float velocity_ = 0.0f; // rad/s + float compensated_velocity_ = 0.0f; // rad/s + float raw_hall_angle_ = 0.0f; + utime_t time_at_last_hall_update_ = 0; + utime_t time_update_last_called_ = 0; + uint64_t number_of_hall_updates_ = 0; + BldcElectricalRotorPositionEstimatorFromHallParams_t* params_ = nullptr; +}; + +} // namespace control_loop + +#endif // BRUSHLESS_FOC_CONTROL_LOOP_HPP diff --git a/inc/comm_manager.hpp b/inc/comm_manager.hpp new file mode 100644 index 0000000..e11e21c --- /dev/null +++ b/inc/comm_manager.hpp @@ -0,0 +1,99 @@ +#ifndef COMM_MANAGER_HPP +#define COMM_MANAGER_HPP + +#include + +#include "comm_message_types.hpp" +#include "hal_serial.hpp" + +namespace comm { +class CommManager { + public: + // TODO: if we want multiple serial ports, we can make this a vector of HAL_SERIAL + CommManager(basilisk_hal::HAL_SERIAL& serial) : serial_(serial) {} + + /** + * @brief Determine the metadata of a packet + * @param buffer The buffer to read from + * @param size The size of the buffer + */ + static comm::generic_message_header_t determine_packet_metadata(uint8_t* buffer, size_t size); + + /** + * @brief Create the metadata for a packet + * @param channel_id The channel ID of the packet + * @param message_length The length of the message + * @param sequence The sequence number of the packet + */ + static comm::generic_message_header_t create_packet_metadata(const comm::ChannelID channel_id, const uint16_t message_length, + const uint8_t sequence); + + /** + * @brief Create a buffer for a message header + * @param header The header to create a buffer for + * @param buffer The buffer to write to + * @param buffer_size The size of the buffer + */ + static bool create_message_header_buffer(const comm::generic_message_header_t header, uint8_t* buffer, + const size_t buffer_size); + + /** + * @brief Subscribe to a message channel and register a callback + * + * @returns Whether the subscription was successful + */ + bool registerCallback(const comm::ChannelID channel_id, void (*callback)(comm::generic_message_header_t, const void*, void*), + void* context); + + /** + * @brief Transmit a message on a given channel + * @param channel_id The channel to transmit on + * @param message The message to transmit + * @returns Whether the transmission was successful + */ + bool sendASAP(const comm::ChannelID channel_id, const comm::Message& message); + + /** + * @brief Given a buffer of data, process it and dispatch it to the appropriate callback + * @param buffer The buffer to process + * @param size The size of the buffer + * @note the buffer should be a complete datagram + */ + void process_received_data(const uint8_t* buffer, const size_t size); + + /**f + * @brief Given a buffer of data, process it and dispatch it to the appropriate callback + * @param buffer The buffer to process + * @param size The size of the buffer + * @param context The context to pass to the callback + */ + static void process_received_data_wrapper(const uint8_t* buffer, const size_t size, void* context) { + CommManager* comm_manager = (CommManager*)context; + if (comm_manager != nullptr) { + comm_manager->process_received_data(buffer, size); + } + } + + // Constants + private: + static constexpr size_t kMaxSubscribers = 32; // Maximum number of subscribers per channel ID + typedef struct { + // Function pointer to the callback + void (*callback)(comm::generic_message_header_t, const void*, void*); + // Context to pass to the callback + void* context; + } subscriber_t; + + // Create an array of subscribers for each channel ID up to the maximum number of subscribers + subscriber_t subscribers_[static_cast(comm::ChannelID::CHANNEL_COUNT)][CommManager::kMaxSubscribers] = {}; + + size_t subscriber_count_[static_cast(comm::ChannelID::CHANNEL_COUNT)] = {}; + basilisk_hal::HAL_SERIAL& serial_; + + // private static buffer for sendASAP messages + uint8_t sendASAP_buffer_[MAX_MESSAGE_BUF_LEN]; +}; + +} // namespace comm + +#endif // COMM_MANAGER_HPP diff --git a/inc/comm_message_types.hpp b/inc/comm_message_types.hpp new file mode 100644 index 0000000..a3d65c2 --- /dev/null +++ b/inc/comm_message_types.hpp @@ -0,0 +1,123 @@ +#ifndef COMM_message_channelS_HPP +#define COMM_message_channelS_HPP +#include + +static const size_t MAX_MESSAGE_BUF_LEN = 256; + +namespace comm { + +using channel_id_t = size_t; + +// Define an enum class for message channel ID's +enum class ChannelID : channel_id_t { + ARM_DISARM = 0, + SET_SPEED = 1, + SYSTEM_MANAGER_STATE = 2, + CHANNEL_COUNT = 3, +}; + +// Define a virtual class for a message +class Message { + public: + Message(uint16_t msg_len) : len(msg_len){}; + ~Message() = default; + /** + * @brief Set the buffer of the message and translate it into the message + * @param buffer The buffer to read from + * @param len The length of the buffer + * @returns Whether the buffer was successfully set + */ + virtual bool populateDataFromBuffer(const uint8_t* buffer, const uint16_t len) = 0; + + /** + * @brief get the buffer of the message for use when sending the message + * @returns The buffer of the message + */ + virtual const void* getBuffer() const = 0; + uint16_t len; +}; + +typedef struct __attribute__((__packed__)) generic_message_header { + uint8_t SOF; + uint16_t message_channel; + uint16_t message_length; + uint8_t message_sequence; +} generic_message_header_t; + +// Define an arm/disarm message +class ArmDisarmMessage : public Message { + public: + typedef struct __attribute__((__packed__)) arm_disarm_message { + bool arm; + } arm_disarm_message_t; + + arm_disarm_message_t data; + ArmDisarmMessage() : Message(sizeof(arm_disarm_message_t)) { data.arm = false; } + ArmDisarmMessage(bool arm) : Message(sizeof(arm_disarm_message_t)) { data.arm = arm; } + const void* getBuffer() const override { return (void*)&data; } + + bool populateDataFromBuffer(const uint8_t* buffer, const uint16_t len) override { + bool ret = false; + if (len == sizeof(arm_disarm_message_t)) { + data = *(arm_disarm_message_t*)buffer; + ret = true; + } + + return ret; + } +}; + +// Define a set speed message +class SetSpeedMessage : public Message { + public: + typedef struct __attribute__((__packed__)) set_speed_message { + float speed; + } set_speed_message_t; + + set_speed_message_t data; + SetSpeedMessage() : Message(sizeof(set_speed_message_t)) { data.speed = 0.0f; } + SetSpeedMessage(float speed) : Message(sizeof(set_speed_message_t)) { data.speed = speed; } + const void* getBuffer() const override { return (void*)&data; } + bool populateDataFromBuffer(const uint8_t* buffer, const uint16_t len) override { + bool ret = false; + if (len == sizeof(set_speed_message_t)) { + data = *(set_speed_message_t*)buffer; + ret = true; + } + + return ret; + } +}; + +// Define a message that describes the state of the SystemManager +class SystemStateMessage : public Message { + public: + typedef struct __attribute__((__packed__)) system_state_message { + bool armed; + float speed; + } system_state_message_t; + + system_state_message_t data; + SystemStateMessage() : Message(sizeof(system_state_message_t)) { + data.armed = false; + data.speed = 0.0f; + } + SystemStateMessage(bool armed, float speed) : Message(sizeof(system_state_message_t)) { + data.armed = armed; + data.speed = speed; + } + const void* getBuffer() const override { return (void*)&data; } + bool populateDataFromBuffer(const uint8_t* buffer, const uint16_t len) override { + bool ret = false; + if (len == sizeof(system_state_message_t)) { + data = *(system_state_message_t*)buffer; + ret = true; + } + + return ret; + } +}; + +} // namespace comm + +#endif // COMM_message_channelS_HPP \ No newline at end of file diff --git a/inc/control_loop.hpp b/inc/control_loop.hpp new file mode 100644 index 0000000..a364c57 --- /dev/null +++ b/inc/control_loop.hpp @@ -0,0 +1,17 @@ +#ifndef CONTROL_LOOP_HPP +#define CONTROL_LOOP_HPP + +namespace control_loop { + +// Make an abstract control loop class with a run method + +class ControlLoop { + public: + virtual void run(float speed) = 0; + // Define a max motor_speed member constant + constexpr static float MAX_MOTOR_SPEED = 1.0f; +}; + +} // namespace control_loop + +#endif // CONTROL_LOOP_HPP \ No newline at end of file diff --git a/inc/example.hpp b/inc/example.hpp new file mode 100644 index 0000000..0434274 --- /dev/null +++ b/inc/example.hpp @@ -0,0 +1,7 @@ +#ifndef EXAMPLE_HPP +#define EXAMPLE_HPP +#include + +int32_t test_add_func(int32_t a, int32_t b); + +#endif diff --git a/inc/hal_adc.hpp b/inc/hal_adc.hpp new file mode 100644 index 0000000..b97b682 --- /dev/null +++ b/inc/hal_adc.hpp @@ -0,0 +1,22 @@ +#ifndef HAL_ADC_HPP +#define HAL_ADC_HPP + +#include + +#include "hal_common.hpp" + +namespace basilisk_hal { +class HAL_ADC { + public: + HAL_ADC() = default; + virtual ~HAL_ADC() {} + /** + * @brief Read the ADC value of a given channel + * @param channel The channel to read from + * @return The voltage value read at the given channel + */ + virtual app_hal_status_E read_adc(float& result, uint8_t channel) = 0; +}; +} // namespace basilisk_hal + +#endif \ No newline at end of file diff --git a/inc/hal_clock.hpp b/inc/hal_clock.hpp new file mode 100644 index 0000000..f698dbd --- /dev/null +++ b/inc/hal_clock.hpp @@ -0,0 +1,26 @@ +#ifndef HAL_CLOCK_HPP +#define HAL_CLOCK_HPP +#include + +#include "hal_common.hpp" + +namespace basilisk_hal { + +// Create a clock class that can be used to get the current time in microseconds via singleton pattern +class HAL_CLOCK { + public: + HAL_CLOCK() = default; + virtual ~HAL_CLOCK() {} + /** + * @brief Get the current time in microseconds + * @return The current time in microseconds + */ + virtual utime_t get_time_us() = 0; + + // Add a constant for the number of microseconds in a second + static constexpr float kMicrosecondsPerSecond = 1000000.0f; +}; + +} // namespace basilisk_hal + +#endif // HAL_CLOCK_HPP \ No newline at end of file diff --git a/inc/hal_common.hpp b/inc/hal_common.hpp new file mode 100644 index 0000000..2cdbae1 --- /dev/null +++ b/inc/hal_common.hpp @@ -0,0 +1,21 @@ +#ifndef HAL_COMMON_HPP +#define HAL_COMMON_HPP + +#include + +typedef enum { + APP_HAL_OK, + APP_HAL_NOT_INITIALIZED, + APP_HAL_BUSY, + APP_HAL_ERROR, + APP_HAL_TIMEOUT, + APP_HAL_NOT_IMPLEMENTED, + APP_HAL_BUFFER_DIMENSION_MISSMATCH, +} app_hal_status_E; + +template +void IGNORE(const T&) {} + +typedef uint64_t utime_t; + +#endif \ No newline at end of file diff --git a/inc/hal_gpio.hpp b/inc/hal_gpio.hpp new file mode 100644 index 0000000..a9280d8 --- /dev/null +++ b/inc/hal_gpio.hpp @@ -0,0 +1,50 @@ +#ifndef HAL_GPIO_HPP +#define HAL_GPIO_HPP + +#include + +#include "hal_common.hpp" + +namespace basilisk_hal { +// Create enum class for GPIO mode (INPUT, OUTPUT, ANALOG, ALTERNATE) +enum class gpio_mode_E { + INPUT, + OUTPUT, + ANALOG, + ALTERNATE, +}; + +class HAL_GPIO { + public: + HAL_GPIO() = default; + virtual ~HAL_GPIO() {} + + /** + * @brief Set the mode of a given GPIO pin + * @param mode The mode to set the pin to + * @return The status of the operation + */ + virtual app_hal_status_E set_mode(gpio_mode_E mode) = 0; + + /** + * @brief Set the output state of a given GPIO pin + * @param pin The pin to set the output state of + * @param state The state to set the pin to + * @return The status of the operation + */ + virtual app_hal_status_E set_output_state(bool state) = 0; + + /** + * @brief Get the input state of a given GPIO pin + * @param pin The pin to get the input state of + * @param state The state to get the pin to + * @return The status of the operation + */ + virtual app_hal_status_E get_input_state(bool& state) = 0; + + protected: + gpio_mode_E mode; +}; +} // namespace basilisk_hal + +#endif \ No newline at end of file diff --git a/inc/hal_serial.hpp b/inc/hal_serial.hpp new file mode 100644 index 0000000..4d7241e --- /dev/null +++ b/inc/hal_serial.hpp @@ -0,0 +1,40 @@ +#ifndef HAL_SERIAL_HPP +#define HAL_SERIAL_HPP + +#include + +#include "hal_common.hpp" + +namespace basilisk_hal { + +// Create a new class for +class HAL_SERIAL { + public: + HAL_SERIAL() = default; + virtual ~HAL_SERIAL() {} + + /** + * @brief Transmit bytes over the serial port + * @param byte Byte array buffer to transmit + * @param size size of buffer + * @return The status of the operation + */ + virtual app_hal_status_E transmit(const void* byte, size_t size) = 0; + + /** + * @brief Receive bytes over the serial port + * @param byte Byte array buffer to receive + * @param size size to receive. If the size is larger than the buffer, the return code will be + * APP_HAL_BUFFER_DIMENSION_MISSMATCH. It will also update the size to the size of data read + * @return The status of the operation + */ + virtual app_hal_status_E receive(void* byte, size_t& size) = 0; + + // TODO: app_hal_status_E async_register_callback which can call comm_manager's process_received_data api + virtual app_hal_status_E async_register_callback(void (*callback)(const uint8_t*, const size_t, void*), + const void* context) = 0; +}; + +} // namespace basilisk_hal + +#endif // HAL_SERIAL \ No newline at end of file diff --git a/inc/hal_timer.hpp b/inc/hal_timer.hpp new file mode 100644 index 0000000..b8aaa08 --- /dev/null +++ b/inc/hal_timer.hpp @@ -0,0 +1,75 @@ +#ifndef HAL_TIMER_HPP +#define HAL_TIMER_HPP +#include + +namespace basilisk_hal { +// Create enum class for timer mode (PWM_EDGE_ALIGNED, PWM_CENTRE_ALIGNED) +enum class timer_mode_E { + PWM_EDGE_ALIGNED, + PWM_CENTRE_ALIGNED, +}; + +// Create enum class for timer channel up to 6 +enum class timer_channel_E { + TIMER_CHANNEL_1 = 1, + TIMER_CHANNEL_2 = 2, + TIMER_CHANNEL_3 = 3, + TIMER_CHANNEL_4 = 4, + TIMER_CHANNEL_5 = 5, + TIMER_CHANNEL_6 = 6, +}; + +class HAL_Timer { + public: + HAL_Timer() = default; + virtual ~HAL_Timer() {} + + // Function to set the parameters of the timer + // TODO: Add more parameters into the function that check for timer channel etc etc + virtual void set_timer_params(timer_mode_E mode, uint32_t frequency) = 0; + + // Function to start the timer + virtual void start() = 0; + + // Function to stop the timer + virtual void stop() = 0; + + // Function to reset the timer + virtual void reset() = 0; + + // Function to set the timer ticks + virtual void set_channel(uint32_t ticks, timer_channel_E channel) = 0; + + // Function to set the timer period + virtual void set_period(uint32_t period) = 0; + + // Function to get the timer period + virtual uint32_t get_period() = 0; + + // Function to start the pwm + virtual void start_pwm(timer_channel_E channel) = 0; +}; + +// Create a complementary PWM timer class +class HAL_ComplementaryPWM_Timer : public HAL_Timer { + public: + HAL_ComplementaryPWM_Timer() = default; + virtual ~HAL_ComplementaryPWM_Timer() {} + + // Declare an ENUM class specifying the complementary channel ACTIVE_LOW or ACTIVE_HIGH + // Note: 'active' refers to when the main PWM is signal is asserted 'high' in most cases. + // Thus, ACTIVE_LOW means that the complementary channel is asserted 'low' when the main PWM is asserted 'high' + // while ACTIVE_HIGH means that the complementary channel is asserted 'high' when the main PWM is asserted 'high' + enum class complementary_PWM_phase_E { + ACTIVE_LOW, + ACTIVE_HIGH, + }; + + // Allow the inverting and non-inverting channels to be used + virtual void set_channel_with_complementary_phase(uint32_t ticks, timer_channel_E channel, + complementary_PWM_phase_E complementary_channel_phase) = 0; +}; + +} // namespace basilisk_hal + +#endif diff --git a/inc/input_manager.hpp b/inc/input_manager.hpp new file mode 100644 index 0000000..f920e88 --- /dev/null +++ b/inc/input_manager.hpp @@ -0,0 +1,50 @@ +#ifndef INPUT_MANAGER_HPP +#define INPUT_MANAGER_HPP +#include "hal_adc.hpp" +#include "hal_common.hpp" + +namespace input_manager { + +class InputManager { + public: + static InputManager& getInstance() { + static InputManager instance; + return instance; + } + + /** + * @brief Register the HAL_ADC object with the input manager + * @param hal_adc The HAL_ADC object to register + * @return None + */ + void register_hal_adc(basilisk_hal::HAL_ADC* hal_adc) { this->hal_adc = hal_adc; } + + /** + * @brief Get the board temperature + * @param temperature The temperature of the PCBA from surface mount sensor + * @return The app hal status + */ + app_hal_status_E get_board_temperature(float& temperature); + + /** + * @brief Get the bus voltage + * @param voltage The voltage of the bus + * @return The app hal status + */ + app_hal_status_E get_bus_voltage(float& voltage); + + /** + * @brief Get the current + * @param current The current going through the motor at time of poll + * @return The app hal status + */ + app_hal_status_E get_motor_current(float& current); + + private: + basilisk_hal::HAL_ADC* hal_adc = nullptr; + InputManager(); +}; + +} // namespace input_manager + +#endif // INPUT_MANAGER_HPP \ No newline at end of file diff --git a/inc/math_foc.hpp b/inc/math_foc.hpp new file mode 100644 index 0000000..58aad7b --- /dev/null +++ b/inc/math_foc.hpp @@ -0,0 +1,144 @@ +#ifndef MATH_CLARKE_PARKE_HPP +#define MATH_CLARKE_PARKE_HPP + +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace math { + +constexpr float sqrt_3 = 1.7320508075688772; +constexpr float sqrt_3_over_3 = 0.5773502691896258; + +constexpr float M_PI_FLOAT = static_cast(M_PI); + +typedef struct { + float alpha; + float beta; +} clarke_transform_result_t; + +typedef struct { + float d; + float q; +} park_transform_result_t; + +typedef struct { + float alpha; + float beta; +} inverse_park_transform_result_t; + +typedef struct { + float a; + float b; + float c; +} inverse_clarke_transform_result_t; + +typedef struct { + float dutyCycleU; + float dutyCycleV; + float dutyCycleW; +} svpwm_duty_cycle_t; + +/** + * @brief Perform a clarke transform on the given 3-phase variable values. + * @param a phase A value to transform to alpha-beta frame + * @param b phase B value to transform to alpha-beta frame + * @param c phase C value to transform to alpha-beta frame + * @return clarke_transform_result_t The result of the clarke transform + */ +clarke_transform_result_t clarke_transform(float a, float b, float c); + +/** + * @brief Perform a park transform on the given alpha-beta values to get d/q frame values. + * @param alpha Current in alpha + * @param beta Current in beta + * @param theta The angle (degrees) of the park transform + * @return park_transform_result_t The result of the park transform + */ +park_transform_result_t park_transform(float alpha, float beta, float theta); + +/** + * @brief Perform an inverse park transform on the given Vq/Vd voltage values. + * @param d parameter in the d direction + * @param q parameter in the q direction + * @param theta The angle (degrees) of the inverse park transform + * @return inverse_park_transform_result_t The result of the inverse park transform + */ +inverse_park_transform_result_t inverse_park_transform(float d, float q, float theta); + +/** + * @brief Perform an inverse clarke transform on the given alpha/beta voltage values. + * @param alpha Voltage in the alpha frame + * @param beta Voltage in the beta frame + * @return inverse_clarke_transform_result_t The result of the inverse clarke transform + */ +inverse_clarke_transform_result_t inverse_clarke_transform(float alpha, float beta); + +/** + * @brief Perform a space vector pulse width modulation on the given alpha/beta voltage values. + * @param Vd Voltage in the d frame + * @param Vq Voltage in the q frame + * @param theta_el The electrical angle of the rotor + * @param Vbus The bus voltage + * @return svpwm_duty_cycle_t The result of the svpwm + */ +svpwm_duty_cycle_t svpwm(float Vd, float Vq, float theta_el, float Vbus); + +/** + * @brief Integrate the given value using the trapezoidal rule. + * @param x The current x value + * @param x_prev The previous x value + * @param y The current y value + * @param y_prev The previous y value + */ +float trapezoidal_integral(float x, float x_prev, float y, float y_prev); + +/** + * @brief Clamp the given value between the given min and max values. + * @param value The value to clamp + * @param min The minimum value + * @param max The maximum value + * @return void + */ +template +void clamp(T& value, const T& min, const T& max) { + if (value < min) { + value = min; + } else if (value > max) { + value = max; + } else { + // do nothing + } +} + +/** + * @brief Wrap the given value around the given min and max values. + * @param value The value to wrap + * @param min The minimum value + * @param max The maximum value + * @return void + */ +template +void wraparound(T& value, const T& min, const T& max) { + while (value < min) { + value += (max - min); + } + while (value > max) { + value -= (max - min); + } +} + +/** + * @brief Low pass filter the given input value. + * @param input The input value + * @param prev_output The previous output value + * @param tau The time constant of the filter + * @param dt The time step + */ +float low_pass_filter(float input, float prev_output, float tau, float dt); + +} // namespace math + +#endif // MATH_CLARKE_PARKE_HPP diff --git a/inc/param_service.hpp b/inc/param_service.hpp new file mode 100644 index 0000000..9bfa826 --- /dev/null +++ b/inc/param_service.hpp @@ -0,0 +1,92 @@ +#ifndef PARAM_SERVER_HPP +#define PARAM_SERVER_HPP +#include + +#include "hal_common.hpp" + +namespace param_service { + +class ParamServer { + public: + static ParamServer& getInstance() { + static ParamServer instance; + return instance; + } + + struct comm_message_ids { + const uint16_t arm_disarm_msg = 0x0001; + const uint16_t set_target_speed_msg = 0x0002; + }; + + // Compile time parameters + struct compile_params { + const bool enable_usb_interface = false; + const bool enable_can_interface = false; + const bool enable_temperature_sense_monitoring = true; + const float overtemperature_threshold_deg_c = 100; + // Analog values + const float analog_reference_voltage = 3.3; + const uint8_t analog_resolution_bits = 10; + const uint8_t temp_sensor_adc_channel = 0; + const uint8_t bus_voltage_adc_channel = 1; + const uint8_t motor_current_adc_channel = 2; + // Thermistor parameters (TODO: make these reflect reality) + const float temp_sensor_volt_offset = 1; + const float temp_sensor_volt_per_deg = 1; + // Bus voltage parameters + const float max_permissable_bus_voltage = 40; + const float bus_voltage_dividor_resistor_1_ohms = 1000; + const float bus_voltage_dividor_resistor_2_ohms = 1000; + // Current sense resistor value for PCBA + const float max_permissable_motor_current_amps = 60; + const float current_sense_resistor_ohms = 0.1; + const float current_sense_gain = 100; + // Control Loop Parameters + const float control_loop_period_s = 0.001; + const float control_loop_deadband = 1; + const float control_loop_max_output = 100; + const float control_loop_min_output = -100; + // H Bridge parameters + const float h_bridge_dead_time_us = 10; + const bool h_bridge_brake_mode_enabled = false; + // Stepper motor parameters + const float stepper_motor_max_speed_steps_per_s = 1000; + const float stepper_motor_max_current_amps = 1.5; + const bool stepper_motor_simple_switcher_enabled = true; + const bool stepper_motor_disable_current_pid = + true; // TODO: remove this. this will use a liner map from the current scales to the pwm duty cycle + const float stepper_motor_current_to_pwm_duty_cycle_slope = 50; + + // 3 phase motor parameters + const utime_t sensorless_phase_commutation_step_time_us = 4000; + const utime_t sensorless_phase_motor_startup_sequence_time_us = sensorless_phase_commutation_step_time_us * 100; + const float sensorless_speed_deadband_scale = 0.3f; // 10% of max speed, lower and we cannot detect back emf + const float sensorless_startup_speed = 1.0f; + const float sensored_speed_deadband_scale = 0.1f; // 10% of max speed, lower and actuating the motor does not make sense + const bool sensorless_bemf_enable_backemf_skip_overrun = false; + const bool log_zero_crossing_in_sensored_mode = false; + const utime_t bemf_zero_crossing_timeout_us = 1875; // time at which we do not detect a ZC after a commutation step + + // FOC parameters + const uint8_t num_hall_updates_to_start = 10; + const utime_t foc_start_timeout_period = 1000000; // 1 second + const float open_loop_velocity_rad_s = 2000.0f; + + // Observer parameters + const float observer_angle_error_kp = 10.0f; + }; + + compile_params compile_params; + + // Get message ids + // Done this way to allow for dynamic message ids + comm_message_ids get_message_ids() { return message_ids; } + + private: + ParamServer(); + comm_message_ids message_ids; +}; + +} // namespace param_service + +#endif diff --git a/inc/pid.hpp b/inc/pid.hpp new file mode 100644 index 0000000..1b33ee9 --- /dev/null +++ b/inc/pid.hpp @@ -0,0 +1,78 @@ +#ifndef PID_HPP +#define PID_HPP +#include "hal_clock.hpp" + +namespace pid { + +// Create a PID controller that takes in a setpoint and a feedback value and returns a control value. +// The class is templated so that it can be used with any type that supports the basic arithmetic operators. +// The class should also take in initial P, I, and D gains as well as a maximum and minimum output value. +// Integral windup should be taken as a construction parameter + +template +class PID { + public: + /** + * @brief Construct a new PID object + * @param kp The proportional gain + * @param ki The integral gain + * @param kd The derivative gain + * @param min_output The maximum output value + * @param max_output The minimum output value + * @param integral_windup The integral windup value + */ + PID(float kp, float ki, float kd, T min_output, T max_output, float integral_windup) + : kp(kp), ki(ki), kd(kd), max_output(max_output), min_output(min_output), integral_windup(integral_windup) { + reset(); + } + + // Reset the PID controller + void reset(); + + // Register a clock object to be used for timing + void register_clock(basilisk_hal::HAL_CLOCK* clock); + + // PID getter functions (marked as const) + float get_kp() const; + float get_ki() const; + float get_kd() const; + + // PID setter functions + void set_kp(float kp); + void set_ki(float ki); + void set_kd(float kd); + + // Set the maximum and minimum output values + void set_max_output(T max_output); + void set_min_output(T min_output); + + // Set the integral windup value + void set_integral_windup(float integral_windup); + + // Calculate the control value given the setpoint and feedback value + /** + * @brief Calculate the control value given the setpoint and feedback value + * @param setpoint The setpoint value + * @param actual The actual value + */ + T calculate(T actual, T setpoint); + + private: + float kp; + float ki; + float kd; + T max_output; + T min_output; + float integral_windup; + // Integral and previous error term storage + float integral; + T previous_error; + + // Clock object to be used for timing + basilisk_hal::HAL_CLOCK* clock = nullptr; + utime_t last_time = 0; +}; + +} // namespace pid + +#endif \ No newline at end of file diff --git a/inc/stepper_control_loop.hpp b/inc/stepper_control_loop.hpp new file mode 100644 index 0000000..d7ade9a --- /dev/null +++ b/inc/stepper_control_loop.hpp @@ -0,0 +1,56 @@ +#ifndef STEPPER_CONTROL_LOOP_HPP +#define STEPPER_CONTROL_LOOP_HPP +#include + +#include "bridge_hbridge.hpp" +#include "control_loop.hpp" +#include "hal_clock.hpp" + +namespace control_loop { + +class StepperControlLoop : public ControlLoop { + public: + // 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; + + // 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) + */ + float determineElectricalAngle(float time_delta, float desired_speed, float previous_angle); + + /** + * @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); + + // Define a function to determine the AB and CD current magnitudes based on the electrical angle + /** + * @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 + */ + std::pair determineCurrentSetpointScalars(float electrical_angle); + + private: + hwbridge::HBridge& motor_a_; + hwbridge::HBridge& motor_b_; + basilisk_hal::HAL_CLOCK& clock_; + float previous_angle_ = 0.0f; + uint32_t previous_time_ = 0; +}; + +} // namespace control_loop + +#endif \ No newline at end of file diff --git a/inc/system_manager.hpp b/inc/system_manager.hpp new file mode 100644 index 0000000..98339c2 --- /dev/null +++ b/inc/system_manager.hpp @@ -0,0 +1,98 @@ +#ifndef SYSTEM_MANAGER_HPP +#define SYSTEM_MANAGER_HPP +#include "comm_manager.hpp" + +namespace system_manager { + +class SystemManager { + public: + static SystemManager& getInstance() { + static SystemManager instance; + return instance; + } + + // Enum containing system manager states + enum class SystemManagerState { + DISABLED, + ENABLED, + FAULT, + }; + + /** + * @brief Run the system manager + */ + void run(); + + /** + * @brief Run the specific system manager state + */ + void runState(SystemManagerState state); + + /** + * @brief Get wheter the controller is enabled or not + */ + bool isEnabled() const; + + /** + * @brief Enable the controller + * TODO: Add flag that permits sending speed as allowing this to enable + */ + + void enableController(); + /** + * @brief Disable the controller + */ + void disableController(); + + void setDesiredSpeed(float speed); + /** + * @brief Check if the system is over temperature + * @param deg_c The temperature in degrees celsius + * @return true if the system is over temperature + */ + bool isOverTemp(float deg_c) const; + + /** + * @brief Check if the system is over voltage + * @param voltage The voltage in volts + * @return true if the system is over voltage + */ + bool isOverVoltage(float voltage) const; + + /** + * @brief Check if the system is over current + * @param current The current in amps + * @return true if the system is over current + */ + bool isOverCurrent(float current) const; + + /** + * @brief Poll the system for faults + */ + void pollFaults(); + + bool is_fault_active{false}; // Whether the system has an active fault or not + + // Get the desired state of the system manager + SystemManagerState getDesiredState(); + + /** + * @brief Get the speed to run the motor at for control loop + * @return The speed to run the motor at. -1.0f -> 1.0f + */ + float getSpeedToRun() const; + + static void receive_set_speed_msg_wrapper(comm::generic_message_header_t header, const void* message, void* context); + + private: + SystemManager(); + SystemManagerState state{SystemManager::SystemManagerState::DISABLED}; + bool controller_enabled{false}; // Controls whether the controller is enabled or not + float desired_speed{0.0f}; // The desired speed to run the motor at + float commanded_speed{0.0f}; // The speed to run the motor at + + void receive_set_speed_msg(comm::generic_message_header_t header, const void* message); +}; +} // namespace system_manager + +#endif \ No newline at end of file diff --git a/libs/googletest b/libs/googletest new file mode 160000 index 0000000..2dd1c13 --- /dev/null +++ b/libs/googletest @@ -0,0 +1 @@ +Subproject commit 2dd1c131950043a8ad5ab0d2dda0e0970596586a diff --git a/scripts/format.sh b/scripts/format.sh new file mode 100644 index 0000000..efe2411 --- /dev/null +++ b/scripts/format.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +cd mains + +# find all C, C++, and header files in the current directory and subdirectories +files=$(find . -name "*.c" -o -name "*.cpp" -o -name "*.h" -o -name "*.hpp") + +# Remove any files located in build/ +files=$(echo "$files" | grep -v "build/") +# Remove any files located in libs/ +files=$(echo "$files" | grep -v "libs/") + +# Loop through the files and format them with clang-format +for file in $files +do + clang-format -i "$file" +done \ No newline at end of file diff --git a/scripts/gdb.sh b/scripts/gdb.sh new file mode 100644 index 0000000..4684434 --- /dev/null +++ b/scripts/gdb.sh @@ -0,0 +1 @@ +gdb build/test/tests \ No newline at end of file diff --git a/scripts/setup.sh b/scripts/setup.sh new file mode 100644 index 0000000..07ad373 --- /dev/null +++ b/scripts/setup.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +set -euxo pipefail + +sudo apt install git clang-format clang-tidy + +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/scripts/test.sh b/scripts/test.sh new file mode 100644 index 0000000..3f1a0b8 --- /dev/null +++ b/scripts/test.sh @@ -0,0 +1,29 @@ +#!/bin/bash + +set -exo pipefail + +# Change to the build directory +cd build + +# Check if the first argument is set +if [ -n "$1" ] +then + # Check if the first argument is "build" or "test" + if [ "$1" == "build" ] + then + # Build the code + make all + elif [ "$1" == "test" ] + then + # Run the tests + ./test/tests + else + # If the argument is not "build" or "test", print an error message + echo "Error: Invalid argument. Valid arguments are 'build', 'test', 'regen-cmake'." + exit 1 + fi +else + # If no argument is specified, build and run the tests + cmake .. -DCMAKE_BUILD_TYPE=Debug -G "Unix Makefiles" + make all && ./test/tests +fi diff --git a/scripts/test_clang_format.sh b/scripts/test_clang_format.sh new file mode 100644 index 0000000..30ce08f --- /dev/null +++ b/scripts/test_clang_format.sh @@ -0,0 +1,30 @@ +#!/bin/bash + +# Find all C, C++, and header files in the current directory +files=$(find . -name "*.c" -o -name "*.cpp" -o -name "*.h" -o -name "*.hpp") + +# Remove any files located in build/ +files=$(echo "$files" | grep -v "build/") +# Remove any files located in libs/ +files=$(echo "$files" | grep -v "libs/") + +# Set a flag to indicate if any files need formatting +need_formatting=0 + +# Loop through the files and check if they need formatting +for file in $files +do + # Check if the file needs formatting + if ! clang-format "$file" | diff "$file" - >/dev/null + then + # The file needs formatting, so set the flag and print an error message + need_formatting=1 + echo "Error: $file needs formatting" + fi +done + +# If any files needed formatting, exit with an error code +if [ "$need_formatting" -eq 1 ] +then + exit 1 +fi \ No newline at end of file diff --git a/src/brushed_control_loop.cpp b/src/brushed_control_loop.cpp new file mode 100644 index 0000000..ea351e4 --- /dev/null +++ b/src/brushed_control_loop.cpp @@ -0,0 +1,95 @@ +#include "brushed_control_loop.hpp" + +#include "param_service.hpp" + +namespace control_loop { + +BrushedControlLoop::BrushedControlLoop() {} + +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; + + // Get the brake mode from the params + bool brake_mode = param_service::ParamServer::getInstance().compile_params.h_bridge_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; +} + +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; + + // 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; + } + } 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; + } + + // 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; + } + + // Check if the time is less than the last speed change time plus the dead time constant + if (current_time_us < + param_service::ParamServer::getInstance().compile_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; + } + + last_motor_speed_ = motor_speed; + + return motor_speed_outputs; +} + +} // namespace control_loop \ No newline at end of file diff --git a/src/brushless_6step_control_loop.cpp b/src/brushless_6step_control_loop.cpp new file mode 100644 index 0000000..a463428 --- /dev/null +++ b/src/brushless_6step_control_loop.cpp @@ -0,0 +1,274 @@ +#include "brushless_6step_control_loop.hpp" + +#include + +#include "param_service.hpp" + +namespace control_loop { + +Brushless6StepControlLoop::Brushless6StepControlLoopState Brushless6StepControlLoop::get_desired_state(utime_t current_time_us, + utime_t time_at_start, + float motor_speed) { + Brushless6StepControlLoop::Brushless6StepControlLoopState return_state = + Brushless6StepControlLoop::Brushless6StepControlLoopState::STOP; + + if (this->rotor_sensor_) { + if (fabs(motor_speed) < param_service::ParamServer::getInstance().compile_params.sensored_speed_deadband_scale) { + return_state = Brushless6StepControlLoop::Brushless6StepControlLoopState::STOP; + } else { + return_state = Brushless6StepControlLoop::Brushless6StepControlLoopState::RUN; + } + } else { + if (fabs(motor_speed) < param_service::ParamServer::getInstance().compile_params.sensorless_speed_deadband_scale) { + return_state = Brushless6StepControlLoop::Brushless6StepControlLoopState::STOP; + } else if (current_time_us - time_at_start < + param_service::ParamServer::getInstance().compile_params.sensorless_phase_motor_startup_sequence_time_us) { + return_state = Brushless6StepControlLoop::Brushless6StepControlLoopState::START; + } else { + return_state = Brushless6StepControlLoop::Brushless6StepControlLoopState::RUN; + } + } + // When stopped, update the time_at_start variable + if (return_state == Brushless6StepControlLoop::Brushless6StepControlLoopState::STOP) { + time_at_start_ = time_at_start; + time_at_start_ = current_time_us; + } + return return_state; +} + +bool Brushless6StepControlLoop::zero_crossing_detected(const hwbridge::Bridge3Phase::bemf_voltage_t& bemf_voltage, + uint8_t commutation_step) { + const commutation_step_t current_commutation_step = commutation_steps[commutation_step]; + float phase_sum = 0.0f; + CommutationSignal zero_crossing_signal = CommutationSignal::Z_RISING; + float undriven_phase_voltage = 0.0f; + + float bemf_voltages[hwbridge::Bridge3Phase::NUM_PHASES] = {bemf_voltage.u, bemf_voltage.v, bemf_voltage.w}; + + for (uint8_t i = 0; i < hwbridge::Bridge3Phase::NUM_PHASES; i++) { + if ((current_commutation_step.signals[i] != CommutationSignal::Z_FALLING) && + (current_commutation_step.signals[i] != CommutationSignal::Z_RISING)) { + phase_sum += bemf_voltages[i]; + } else { + zero_crossing_signal = current_commutation_step.signals[i]; + undriven_phase_voltage = bemf_voltages[i]; + } + } + + float zero_crossing_threshold = phase_sum / 2.0f; // NOTE: This requires the bemf voltage to run when the PWM is ON + bool return_value = false; + if (zero_crossing_signal == CommutationSignal::Z_RISING) { + if (undriven_phase_voltage > zero_crossing_threshold) { + return_value = true; + } + } else { + if (undriven_phase_voltage < zero_crossing_threshold) { + return_value = true; + } + } + + return return_value; +} + +void Brushless6StepControlLoop::run(float speed) { + hwbridge::Bridge3Phase::phase_command_t phase_commands[3] = {0, 0}; + + // Get the desired motor speed from the system manager + float desired_motor_speed = speed; + + // Get the current time + utime_t current_time_us = this->clock_.get_time_us(); + + // Get the desired state of the control loop + Brushless6StepControlLoop::Brushless6StepControlLoopState desired_state = + get_desired_state(current_time_us, time_at_start_, desired_motor_speed); + + if (desired_state != state_) { + switch (desired_state) { + case Brushless6StepControlLoopState::STOP: { + commutation_step_ = 0; + motor_speed_ = 0; + break; + } + case Brushless6StepControlLoopState::START: { + last_commutation_step_switch_time_ = current_time_us; + break; + } + case Brushless6StepControlLoopState::RUN: { + break; + } + default: { + break; + } + } + state_ = desired_state; + } + + // Switch case on states: + switch (state_) { + case Brushless6StepControlLoopState::STOP: { + for (int i = 0; i < hwbridge::Bridge3Phase::NUM_PHASES; i++) { + phase_commands[i].duty_cycle_high_side = 0.0f; + phase_commands[i].invert_low_side = false; + } + this->zero_crossing_time_ = 0; + break; + } + case Brushless6StepControlLoopState::START: { + // This defines the startup sequence for the motor + // Use the deadband scale to determine the speed to run the motor at + float startup_motor_speed = param_service::ParamServer::getInstance().compile_params.sensorless_startup_speed; + // Check if we should switch to the next commutation step + if (current_time_us - last_commutation_step_switch_time_ > + param_service::ParamServer::getInstance().compile_params.sensorless_phase_commutation_step_time_us) { + // Switch to the next commutation step + commutation_step_++; + if (commutation_step_ > 5) { + commutation_step_ = 0; + } + last_commutation_step_switch_time_ = current_time_us; + } + + // Compute the phase commands + generate_commutation_duty_cycles(phase_commands, commutation_step_, startup_motor_speed); + break; + } + case Brushless6StepControlLoopState::RUN: { + motor_speed_ = desired_motor_speed; + // Check first if a rotor is defined + if (this->rotor_sensor_) { + uint8_t sector = 0; + // TODO: Handle failure if HAL fails + rotor_sensor_->get_sector(sector); + if (sector != commutation_step_) { + this->update_average_commutation_step_delta_time(current_time_us); + commutation_step_ = sector; + zero_crossing_time_ = 0; + } + + // Log zerocrossings if enabled in param server + if (param_service::ParamServer::getInstance().compile_params.log_zero_crossing_in_sensored_mode) { + // Get the bemf voltage + hwbridge::Bridge3Phase::bemf_voltage_t bemf_voltage; + this->motor_.read_bemf(bemf_voltage); + // If the desired motor speed is negative, we need to swap the v and w phase bemf readings + if (motor_speed_ < 0) { + float temp = bemf_voltage.v; + bemf_voltage.v = bemf_voltage.w; + bemf_voltage.w = temp; + } + // Check if we have detected a zero crossing. Only update the variable if the ZCT is 0 + if (zero_crossing_detected(bemf_voltage, commutation_step_) && zero_crossing_time_ == 0 && + (current_time_us - last_commutation_step_switch_time_) > + param_service::ParamServer::getInstance().compile_params.bemf_zero_crossing_timeout_us) { + zero_crossing_time_ = current_time_us; + } + } + + } else { + // Check if we should switch to the next commutation step + // First, check if we have detected a zero crossing on the bemf voltage + if (zero_crossing_time_ == 0) { + // Get the bemf voltage + hwbridge::Bridge3Phase::bemf_voltage_t bemf_voltage; + this->motor_.read_bemf(bemf_voltage); + // If the desired motor speed is negative, we need to swap the v and w phase bemf readings + if (motor_speed_ < 0) { + float temp = bemf_voltage.v; + bemf_voltage.v = bemf_voltage.w; + bemf_voltage.w = temp; + } + // Check if we have detected a zero crossing + // If we don't detect a zero crossing, look at the average commutation step time to see if we should switch + if (zero_crossing_detected(bemf_voltage, commutation_step_) && + (current_time_us - last_commutation_step_switch_time_) > + param_service::ParamServer::getInstance().compile_params.bemf_zero_crossing_timeout_us) { + zero_crossing_time_ = current_time_us; + } else if (average_commutation_step_delta_time_ != 0 && + current_time_us - last_commutation_step_switch_time_ > average_commutation_step_delta_time_ && + param_service::ParamServer::getInstance() + .compile_params.sensorless_bemf_enable_backemf_skip_overrun) { + // Switch to the next commutation step + commutation_step_switch_sensorless(current_time_us); + } + } else { + // Check if we should switch to the next commutation step + if (current_time_us > + 2 * (zero_crossing_time_ - last_commutation_step_switch_time_) + last_commutation_step_switch_time_) { + // Switch to the next commutation step + commutation_step_switch_sensorless(current_time_us); + } + } + } + + // Compute the phase commands + generate_commutation_duty_cycles(phase_commands, commutation_step_, motor_speed_); + + break; + } + } + + // Send the phase commands to the bridge + // if the desired motor speed is negative, we need to swap the u, v, and w phase commands + if (motor_speed_ < 0) { + hwbridge::Bridge3Phase::phase_command_t temp = phase_commands[2]; + phase_commands[2] = phase_commands[1]; + phase_commands[1] = phase_commands[0]; + phase_commands[0] = temp; + } + this->motor_.set_phase(phase_commands[0], phase_commands[1], phase_commands[2]); +} + +void Brushless6StepControlLoop::generate_commutation_duty_cycles(hwbridge::Bridge3Phase::phase_command_t phase_command[3], + uint8_t commutation_step, float motor_speed) { + const commutation_step_t current_commutation_step = commutation_steps[commutation_step]; + for (int i = 0; i < 3; i++) { + if (current_commutation_step.signals[i] == CommutationSignal::HIGH) { + phase_command[i].duty_cycle_high_side = fabs(motor_speed); + phase_command[i].invert_low_side = true; + } else if (current_commutation_step.signals[i] == CommutationSignal::LOW) { + phase_command[i].duty_cycle_high_side = 0.0f; + phase_command[i].invert_low_side = true; + } else { + phase_command[i].duty_cycle_high_side = 0.0f; + phase_command[i].invert_low_side = false; + } + } +} + +void Brushless6StepControlLoop::commutation_step_switch_sensorless(utime_t current_time_us) { + // Switch to the next commutation step + commutation_step_++; + // update the average commutation step delta time + this->update_average_commutation_step_delta_time(current_time_us); + if (commutation_step_ > 5) { + commutation_step_ = 0; + } + // Reset the zero crossing time + zero_crossing_time_ = 0; +} + +utime_t Brushless6StepControlLoop::calculate_average_commutation_step_delta_time( + utime_t new_commutation_step_delta_time, utime_t* average_commutation_step_delta_time_array, size_t size) { + // Calculate the average commutation step delta time + utime_t average_commutation_step_delta_time = 0; + for (size_t i = 0; i < size; i++) { + average_commutation_step_delta_time += average_commutation_step_delta_time_array[i]; + } + average_commutation_step_delta_time /= size; + + // Shift the array + for (size_t i = size - 1; i > 0; i--) { + average_commutation_step_delta_time_array[i] = average_commutation_step_delta_time_array[i - 1]; + } + average_commutation_step_delta_time_array[0] = new_commutation_step_delta_time; + + // Check that the last element is not zero + if (average_commutation_step_delta_time_array[size - 1] == 0) { + average_commutation_step_delta_time = 0; + } + + return average_commutation_step_delta_time; +} + +} // namespace control_loop diff --git a/src/brushless_foc_control_loop.cpp b/src/brushless_foc_control_loop.cpp new file mode 100644 index 0000000..1a851d0 --- /dev/null +++ b/src/brushless_foc_control_loop.cpp @@ -0,0 +1,338 @@ +#include "brushless_foc_control_loop.hpp" + +#include "math.h" +#include "math_foc.hpp" + +namespace control_loop { + +// Define the init function +void BrushlessFocControlLoop::init(BrushlessFocControlLoop::BrushlessFocControLoopParams* params) { + // Initialize the rotor position estimator + rotor_position_estimator_.reset_estimation(); + + // Set the internal params pointer + params_ = params; + + // Initialize the PID controllers + pid_d_current_.set_kd(params_->kd_d_current); + pid_d_current_.set_ki(params_->ki_d_current); + pid_d_current_.set_kp(params_->kp_d_current); + + pid_q_current_.set_kd(params_->kd_q_current); + pid_q_current_.set_ki(params_->ki_q_current); + pid_q_current_.set_kp(params_->kp_q_current); + + // Reset the PID controllers + pid_d_current_.reset(); + pid_q_current_.reset(); +} + +BrushlessFocControlLoop::BrushlessFocControlLoopState BrushlessFocControlLoop::get_desired_state( + float motor_speed, const BrushlessFocControlLoopState current_state) { + BrushlessFocControlLoop::BrushlessFocControlLoopState desired_state = current_state; + switch (current_state) { + case BrushlessFocControlLoop::BrushlessFocControlLoopState::NOT_INITIALIZED: { + if (params_ != nullptr) { + desired_state = BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP; + } + } break; + case BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP: { + // if the estimator reports that it is valid, then we should start the motor + if ((motor_speed != 0)) { + desired_state = BrushlessFocControlLoop::BrushlessFocControlLoopState::RUN; + } + } break; + case BrushlessFocControlLoop::BrushlessFocControlLoopState::RUN: { + if (motor_speed == 0.0f) { + desired_state = BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP; + } + } break; + default: + // Unknown state + break; + } + + return desired_state; +} + +void BrushlessFocControlLoop::run(float speed) { + // Get the current time + utime_t current_time_us = clock_.get_time_us(); + + hwbridge::Bridge3Phase::phase_command_t phase_commands[3] = {0, false}; + + // Update the rotor position estimator + rotor_position_estimator_.update(current_time_us); + + // Get the current state and the desired state + BrushlessFocControlLoop::BrushlessFocControlLoopState desired_state = get_desired_state(speed, state_); + + // If the desired state is different from the current state, then we need to transition + if (desired_state != state_) { + switch (desired_state) { + case BrushlessFocControlLoop::BrushlessFocControlLoopState::RUN: { + // reset the PID controllers + pid_d_current_.reset(); + pid_q_current_.reset(); + // reset the rotor position estimator + rotor_position_estimator_.reset_estimation(); + // Set the desired rotor angle to the current rotor angle + rotor_position_estimator_.get_rotor_position(desired_rotor_angle_open_loop_); + } break; + case BrushlessFocControlLoop::BrushlessFocControlLoopState::NOT_INITIALIZED: + case BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP: + default: + break; + } + state_ = desired_state; + } + + // Run the state machine + switch (state_) { + case BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP: + break; + + case BrushlessFocControlLoop::BrushlessFocControlLoopState::RUN: { + // Get the bus voltage + float bus_voltage = 0.0f; + // TODO: ERROR CHECKING!! + bridge_.read_bus_voltage(bus_voltage); + + // Determine the control loop type + const BrushlessFocControlLoopType desired_control_loop_type = + get_desired_control_loop_type(rotor_position_estimator_.is_estimation_valid()); + switch (desired_control_loop_type) { + case BrushlessFocControlLoopType::OPEN_LOOP: { + V_quadrature_ = speed * bus_voltage; + V_direct_ = 0.0f; // TODO: add param for open-loop direct voltage + + // increment the rotor position by the speed multiplied by the time since the last run + desired_rotor_angle_open_loop_ += params_->open_loop_full_speed_theta_velocity * speed * + (float)(current_time_us - last_run_time_) / + basilisk_hal::HAL_CLOCK::kMicrosecondsPerSecond; + // Wrap the rotor position around 0 and 2pi + math::wraparound(desired_rotor_angle_open_loop_, 0.0f, float(2.0f * M_PI)); + + rotor_position_ = desired_rotor_angle_open_loop_; + + } break; + case BrushlessFocControlLoopType::CLOSED_LOOP: { + // Get the FOC current + hwbridge::Bridge3Phase::phase_current_t phase_currents; + bridge_.read_current(phase_currents); + + rotor_position_estimator_.get_rotor_position(rotor_position_); + + // Do a Clarke transform + math::clarke_transform_result_t clarke_transform = + math::clarke_transform(phase_currents.u, phase_currents.v, phase_currents.w); + + // Do a Park transform + math::park_transform_result_t park_transform_currents = + math::park_transform(clarke_transform.alpha, clarke_transform.beta, rotor_position_); + + i_direct_ = park_transform_currents.d; + i_quadrature_ = park_transform_currents.q; + + // Run the PI controller + // The below hack for speed is kinda hacky and should be reverted lol + V_quadrature_ += pid_q_current_.calculate(i_quadrature_, speed * params_->speed_to_iq_gain); + V_direct_ += pid_d_current_.calculate(i_direct_, params_->i_d_reference); + } break; + default: + break; + } + // Limit the Vd and Vq + math::clamp(V_direct_, -bus_voltage, bus_voltage); + math::clamp(V_quadrature_, -bus_voltage, bus_voltage); + + // Determine the appropriate duty cycles for the inverter + determine_inverter_duty_cycles(rotor_position_, V_direct_, V_quadrature_, bus_voltage, params_->pwm_control_type, + phase_commands[0], phase_commands[1], phase_commands[2]); + } break; + + default: + break; + } + + // Set the duty cycles + this->bridge_.set_phase(phase_commands[0], phase_commands[1], phase_commands[2]); + + last_run_time_ = current_time_us; +} + +void BrushlessFocControlLoop::determine_inverter_duty_cycles(float theta, float Vdirect, float Vquadrature, float bus_voltage, + BrushlessFocControlLoop::BrushlessFocPwmControlType pwm_control_type, + hwbridge::Bridge3Phase::phase_command_t& phase_command_u, + hwbridge::Bridge3Phase::phase_command_t& phase_command_v, + hwbridge::Bridge3Phase::phase_command_t& phase_command_w) { + switch (pwm_control_type) { + case BrushlessFocControlLoop::BrushlessFocPwmControlType::SPACE_VECTOR: { + math::svpwm_duty_cycle_t duty_cycles = math::svpwm(Vdirect, Vquadrature, theta, bus_voltage); + duty_cycle_u_h_ = duty_cycles.dutyCycleU; + duty_cycle_v_h_ = duty_cycles.dutyCycleV; + duty_cycle_w_h_ = duty_cycles.dutyCycleW; + } break; + case BrushlessFocControlLoop::BrushlessFocPwmControlType::SINE: { + // Do an inverse Park transform + math::inverse_park_transform_result_t inverse_park_transform = + math::inverse_park_transform(Vdirect, Vquadrature, theta); + + V_alpha_ = inverse_park_transform.alpha; + V_beta_ = inverse_park_transform.beta; + + // Do an inverse clarke transform + math::inverse_clarke_transform_result_t inverse_clarke_transform = + math::inverse_clarke_transform(inverse_park_transform.alpha, inverse_park_transform.beta); + + // load the results into the phase commands + duty_cycle_u_h_ = inverse_clarke_transform.a / bus_voltage; + duty_cycle_v_h_ = inverse_clarke_transform.b / bus_voltage; + duty_cycle_w_h_ = inverse_clarke_transform.c / bus_voltage; + + // Duty cycles can be between -1 and 1, and those should linearly map to 0 -> 1 + duty_cycle_u_h_ = (duty_cycle_u_h_ + this->MAX_MOTOR_SPEED) / (this->MAX_MOTOR_SPEED * 2.0f); + duty_cycle_v_h_ = (duty_cycle_v_h_ + this->MAX_MOTOR_SPEED) / (this->MAX_MOTOR_SPEED * 2.0f); + duty_cycle_w_h_ = (duty_cycle_w_h_ + this->MAX_MOTOR_SPEED) / (this->MAX_MOTOR_SPEED * 2.0f); + } break; + default: + // Set the duty cycles to 0 + duty_cycle_u_h_ = 0.0f; + duty_cycle_v_h_ = 0.0f; + duty_cycle_w_h_ = 0.0f; + break; + } + + // Set the duty cycles + phase_command_u.duty_cycle_high_side = duty_cycle_u_h_; + phase_command_u.invert_low_side = true; + phase_command_v.duty_cycle_high_side = duty_cycle_v_h_; + phase_command_v.invert_low_side = true; + phase_command_w.duty_cycle_high_side = duty_cycle_w_h_; + phase_command_w.invert_low_side = true; +} + +BrushlessFocControlLoop::BrushlessFocControlLoopType BrushlessFocControlLoop::get_desired_control_loop_type( + bool is_estimator_valid) { + BrushlessFocControlLoop::BrushlessFocControlLoopType desired_control_loop_type = + BrushlessFocControlLoop::BrushlessFocControlLoopType::OPEN_LOOP; + if (is_estimator_valid) { + desired_control_loop_type = BrushlessFocControlLoop::BrushlessFocControlLoopType::CLOSED_LOOP; + } + return desired_control_loop_type; +} + +app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::reset_estimation() { + app_hal_status_E ret = APP_HAL_OK; + + rotor_position_ = 0.0f; + velocity_ = 0.0f; + compensated_velocity_ = 0.0f; + raw_hall_angle_ = 0.0f; + time_at_last_hall_update_ = 0; + time_update_last_called_ = 0; + number_of_hall_updates_ = 0; + + // Update the rotor position with the sector sensor + uint8_t sector = 0; + ret = sector_sensor_.get_sector(sector); + if (ret == APP_HAL_OK) { + rotor_position_ = sector * 2.0 * M_PI / 6.0f; + } + + return ret; +} + +bool BldcElectricalRotorPositionEstimatorFromHall::is_estimation_valid() { + bool ret = false; + if (params_ != nullptr) { + bool num_hall_updates_to_start = (number_of_hall_updates_ > params_->num_hall_updates_to_start); + // Also make the return conditional if the position estimate no greater than the param for tolerance + bool rotor_position_tolerance = (fabs(rotor_position_ - raw_hall_angle_) < params_->max_estimate_angle_overrun); + ret = num_hall_updates_to_start && rotor_position_tolerance; + } + return ret; +} + +app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::init( + BldcElectricalRotorPositionEstimatorFromHallParams_t* params) { + app_hal_status_E ret = APP_HAL_OK; + + // Set the internal params pointer + params_ = params; + + return ret; +} + +app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::update(utime_t time) { + app_hal_status_E ret = APP_HAL_OK; + do { + if (params_ == nullptr) { + ret = APP_HAL_ERROR; + break; + } + + uint8_t sector = 0; + + ret = sector_sensor_.get_sector(sector); + + if (ret != APP_HAL_OK) { + break; + } + + float raw_hall_angle = sector * 2 * M_PI / 6; + // We should not update velocity estimations if the raw hall angle is the same as the previous one + if (fabs(raw_hall_angle - raw_hall_angle_) > 0.001f) { + // Calculate velocity with a simple differentiation + const float time_delta_since_hall_update = + (float)(time - time_at_last_hall_update_) / basilisk_hal::HAL_CLOCK::kMicrosecondsPerSecond; + + float raw_hall_angle_diff = raw_hall_angle - raw_hall_angle_; + + // Account for over/underflow + // NOTE: if the rotor delta theta is greater than pi radians, then this detection will not work + math::wraparound(raw_hall_angle_diff, static_cast(-M_PI), static_cast(M_PI)); + + velocity_ = raw_hall_angle_diff / time_delta_since_hall_update; + + // Calculate a compensated velocity to account for position error and smoothly compensate for it + compensated_velocity_ = velocity_; + // TODO: implement compensated_velocity_ = velocity_ * (1 - ((rotor_position_ - raw_hall_angle_diff) / + // raw_hall_angle_diff)); + + rotor_position_ = raw_hall_angle; + this->raw_hall_angle_ = raw_hall_angle; + time_at_last_hall_update_ = time; + number_of_hall_updates_++; + } + + const float current_measurement_period = + (float)(time - time_update_last_called_) / basilisk_hal::HAL_CLOCK::kMicrosecondsPerSecond; + // Update the rotor position with the velocity estimate + rotor_position_ += compensated_velocity_ * current_measurement_period; + + // Implement a wraparound + math::wraparound(rotor_position_, 0.0f, float(2.0f * M_PI)); + + time_update_last_called_ = time; + } while (false); + return ret; +} + +app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::get_rotor_position(float& rotor_position) { + app_hal_status_E ret = APP_HAL_OK; + + rotor_position = rotor_position_; + + return ret; +} + +app_hal_status_E BldcElectricalRotorPositionEstimatorFromHall::get_rotor_velocity(float& rotor_velocity) { + app_hal_status_E ret = APP_HAL_OK; + + rotor_velocity = compensated_velocity_; + + return ret; +} + +} // namespace control_loop diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp new file mode 100644 index 0000000..af0c83a --- /dev/null +++ b/src/comm_manager.cpp @@ -0,0 +1,119 @@ +#include "comm_manager.hpp" + +#include + +#include "hal_common.hpp" + +namespace comm { + +generic_message_header_t CommManager::determine_packet_metadata(uint8_t* buffer, size_t size) { + generic_message_header_t header = {0x00, 0x0000, 0x0000, 0x00}; + if (size >= 4) { + header.SOF = buffer[0]; + header.message_channel = (buffer[1] << 8) | buffer[2]; + header.message_length = (buffer[3] << 8) | buffer[4]; + header.message_sequence = buffer[5]; + } + return header; +} + +generic_message_header_t CommManager::create_packet_metadata(const comm::ChannelID channel_id, const uint16_t message_length, + const uint8_t sequence) { + generic_message_header_t header; + header.SOF = 0x00; + header.message_channel = static_cast(channel_id); + header.message_length = message_length; + header.message_sequence = sequence; + + return header; +} + +bool CommManager::create_message_header_buffer(const comm::generic_message_header_t header, uint8_t* buffer, + const size_t buffer_size) { + bool ret = false; + if (buffer_size >= sizeof(generic_message_header_t)) { + buffer[0] = header.SOF; + buffer[1] = header.message_channel >> 8; + buffer[2] = header.message_channel & 0xFF; + buffer[3] = header.message_length >> 8; + buffer[4] = header.message_length & 0xFF; + buffer[5] = header.message_sequence; + ret = true; + } + return ret; +} + +bool CommManager::registerCallback(const comm::ChannelID channel_id, + void (*callback)(comm::generic_message_header_t, const void*, void*), void* context) { + bool ret = false; + // get the number of subscribers for the channel + size_t num_subscribers = subscriber_count_[static_cast(channel_id)]; + // check if the number of subscribers is less than the max + if (num_subscribers < kMaxSubscribers) { + // add the callback to the subscribers array + subscriber_t* subscriber_ = &subscribers_[static_cast(channel_id)][num_subscribers]; + subscriber_->callback = callback; + subscriber_->context = context; + // increment the number of subscribers + subscriber_count_[static_cast(channel_id)]++; + ret = true; + } + + return ret; +} + +void CommManager::process_received_data(const uint8_t* buffer, const size_t size) { + size_t i = 0; + while (i < size) { + // get the header + generic_message_header_t header = determine_packet_metadata((uint8_t*)buffer + i, size - i); + // get the channel id + channel_id_t channel_id = static_cast(header.message_channel); + // get the message length + uint16_t message_length = header.message_length; + // get the sequence + uint8_t sequence = header.message_sequence; + (void)(sequence); // UNUSED + + if (message_length > size - i) { + // the message length is too long, so break out of the loop + break; + } else if (channel_id >= static_cast(ChannelID::CHANNEL_COUNT)) { + // the channel id is invalid, so break out of the loop + break; + } + + // Fire the callback for the channel + for (size_t j = 0; j < subscriber_count_[static_cast(channel_id)]; j++) { + subscriber_t* subscriber_ = &subscribers_[static_cast(channel_id)][j]; + subscriber_->callback(header, buffer + i + sizeof(generic_message_header_t), subscriber_->context); + } + + // increment i by the message length and the size of the header + i += message_length + sizeof(generic_message_header_t); + } +} + +bool CommManager::sendASAP(const comm::ChannelID channel_id, const comm::Message& message) { + bool ret = false; + // get the buffer + const void* buffer = message.getBuffer(); + // get the length + uint16_t len = message.len; + + // create the header + generic_message_header_t header = CommManager::create_packet_metadata(channel_id, len, 0U); + + // copy the header into the buffer + memcpy(sendASAP_buffer_, &header, sizeof(generic_message_header_t)); + // copy the message into the buffer + memcpy(sendASAP_buffer_ + sizeof(generic_message_header_t), buffer, len); + + // transmit the buffer + if (serial_.transmit(sendASAP_buffer_, len + sizeof(generic_message_header_t)) == APP_HAL_OK) { + ret = true; + } + return ret; +} + +} // namespace comm \ No newline at end of file diff --git a/src/example.cpp b/src/example.cpp new file mode 100644 index 0000000..5563875 --- /dev/null +++ b/src/example.cpp @@ -0,0 +1,5 @@ +#include "example.hpp" + +#include + +int32_t test_add_func(int32_t a, int32_t b) { return a + b; } diff --git a/src/input_manager.cpp b/src/input_manager.cpp new file mode 100644 index 0000000..b85f46a --- /dev/null +++ b/src/input_manager.cpp @@ -0,0 +1,55 @@ +#include "input_manager.hpp" + +#include "param_service.hpp" + +namespace input_manager { + +InputManager::InputManager() {} + +app_hal_status_E InputManager::get_board_temperature(float& temperature) { + app_hal_status_E returnVal = APP_HAL_OK; + if (this->hal_adc == nullptr) { + return APP_HAL_NOT_INITIALIZED; + } + float voltage = 0; + returnVal = + this->hal_adc->read_adc(voltage, param_service::ParamServer::getInstance().compile_params.temp_sensor_adc_channel); + if (returnVal == APP_HAL_OK) { + temperature = (voltage - param_service::ParamServer::getInstance().compile_params.temp_sensor_volt_offset) / + param_service::ParamServer::getInstance().compile_params.temp_sensor_volt_per_deg; + } + return returnVal; +} + +app_hal_status_E InputManager::get_bus_voltage(float& voltage) { + app_hal_status_E returnVal = APP_HAL_OK; + if (this->hal_adc == nullptr) { + return APP_HAL_NOT_INITIALIZED; + } + returnVal = + this->hal_adc->read_adc(voltage, param_service::ParamServer::getInstance().compile_params.bus_voltage_adc_channel); + if (returnVal == APP_HAL_OK) { + voltage = voltage * + (param_service::ParamServer::getInstance().compile_params.bus_voltage_dividor_resistor_1_ohms + + param_service::ParamServer::getInstance().compile_params.bus_voltage_dividor_resistor_2_ohms) / + param_service::ParamServer::getInstance().compile_params.bus_voltage_dividor_resistor_2_ohms; + } + return returnVal; +} + +app_hal_status_E InputManager::get_motor_current(float& current) { + app_hal_status_E returnVal = APP_HAL_OK; + if (this->hal_adc == nullptr) { + return APP_HAL_NOT_INITIALIZED; + } + float voltage = 0; + returnVal = + this->hal_adc->read_adc(voltage, param_service::ParamServer::getInstance().compile_params.motor_current_adc_channel); + if (returnVal == APP_HAL_OK) { + current = voltage / param_service::ParamServer::getInstance().compile_params.current_sense_resistor_ohms / + param_service::ParamServer::getInstance().compile_params.current_sense_gain; + } + return returnVal; +} + +} // namespace input_manager \ No newline at end of file diff --git a/src/math_foc.cpp b/src/math_foc.cpp new file mode 100644 index 0000000..80c6546 --- /dev/null +++ b/src/math_foc.cpp @@ -0,0 +1,117 @@ +#include "math_foc.hpp" + +#include "math.h" + +namespace math { + +clarke_transform_result_t clarke_transform(float a, float b, float c) { + clarke_transform_result_t result; + result.alpha = a; + result.beta = (b - c) / sqrt_3; + return result; +} + +park_transform_result_t park_transform(float alpha, float beta, float theta) { + park_transform_result_t result; + float theta_rad = theta; + float cos_theta = cos(theta_rad); + float sin_theta = sin(theta_rad); + result.d = alpha * cos_theta + beta * sin_theta; + result.q = -alpha * sin_theta + beta * cos_theta; + return result; +} + +inverse_park_transform_result_t inverse_park_transform(float d, float q, float theta) { + inverse_park_transform_result_t result; + float theta_rad = theta; + float cos_theta = cos(theta_rad); + float sin_theta = sin(theta_rad); + result.alpha = d * cos_theta - q * sin_theta; + result.beta = d * sin_theta + q * cos_theta; + return result; +} + +inverse_clarke_transform_result_t inverse_clarke_transform(float alpha, float beta) { + inverse_clarke_transform_result_t result; + result.a = alpha; + result.b = (-alpha + sqrt_3 * beta) / 2.0f; + result.c = (-alpha - sqrt_3 * beta) / 2.0f; + return result; +} + +svpwm_duty_cycle_t svpwm(float Vd, float Vq, float theta_el, float Vbus) { + svpwm_duty_cycle_t result = {0.0f, 0.0f, 0.0f}; + // First, calcuate the magnitude of the voltage vector + float Vmag = sqrtf(Vd * Vd + Vq * Vq); + // Now, normalize the Vmag with the bus voltage + float Vmag_norm = Vmag / Vbus; + // Calculate the working theta that further adds the arctan of the voltage vector + float svpwm_theta = theta_el + atan2f(Vq, Vd); + // Wrap the working theta to be between 0 and 2pi + math::wraparound(svpwm_theta, 0.0f, 2.0f * M_PI_FLOAT); + // Calculate the sector + // NOTE: This is a 1-indexed sector as 0 and 7 are reseved as null sectors + uint8_t sector = static_cast(svpwm_theta / (M_PI_FLOAT / 3.0f)) + 1; + + // Calculate the time T0,T1,T2 by using the normalized voltage magnitude + // See https://www.youtube.com/watch?v=QMSWUMEAejg for the derivations of the equations below + // Note: Tz (or total period) is always 1.0f + // Vref/Vdc is accounted for with our Vmag_norm calculation above + float T1 = sqrt_3 * (sector * M_PI_FLOAT / 3.0f - svpwm_theta) * Vmag_norm; + float T2 = sqrt_3 * (svpwm_theta - (sector - 1) * M_PI_FLOAT / 3.0f) * Vmag_norm; + float T0 = 1.0f - T1 - T2; + + // Now, calculate the duty cycles based on the sector + switch (sector) { + case 1: + result.dutyCycleU = T1 + T2 + T0 / 2.0f; + result.dutyCycleV = T2 + T0 / 2.0f; + result.dutyCycleW = T0 / 2.0f; + break; + case 2: + result.dutyCycleU = T1 + T0 / 2.0f; + result.dutyCycleV = T1 + T2 + T0 / 2.0f; + result.dutyCycleW = T0 / 2.0f; + break; + case 3: + result.dutyCycleU = T0 / 2.0f; + result.dutyCycleV = T1 + T2 + T0 / 2.0f; + result.dutyCycleW = T2 + T0 / 2.0f; + break; + case 4: + result.dutyCycleU = T0 / 2.0f; + result.dutyCycleV = T1 + T0 / 2.0f; + result.dutyCycleW = T1 + T2 + T0 / 2.0f; + break; + case 5: + result.dutyCycleU = T2 + T0 / 2.0f; + result.dutyCycleV = T0 / 2.0f; + result.dutyCycleW = T1 + T2 + T0 / 2.0f; + break; + case 6: + result.dutyCycleU = T1 + T2 + T0 / 2.0f; + result.dutyCycleV = T0 / 2.0f; + result.dutyCycleW = T1 + T0 / 2.0f; + break; + default: + // wrong parta' down buddy (as an old boss would say) + result.dutyCycleU = 0.0f; + result.dutyCycleV = 0.0f; + result.dutyCycleW = 0.0f; + break; + } + + return result; +} + +float trapezoidal_integral(float x, float x_prev, float y, float y_prev) { + float integral = (x - x_prev) * (y + y_prev) / 2.0f; + return integral; +} + +float low_pass_filter(float input, float prev_output, float tau, float dt) { + float alpha = dt / (tau + dt); + return alpha * input + (1.0f - alpha) * prev_output; +} + +} // namespace math \ No newline at end of file diff --git a/src/param_service.cpp b/src/param_service.cpp new file mode 100644 index 0000000..5e2a56c --- /dev/null +++ b/src/param_service.cpp @@ -0,0 +1,7 @@ +#include "param_service.hpp" + +namespace param_service { + +ParamServer::ParamServer() {} + +} // namespace param_service \ No newline at end of file diff --git a/src/pid.cpp b/src/pid.cpp new file mode 100644 index 0000000..0c9aed6 --- /dev/null +++ b/src/pid.cpp @@ -0,0 +1,109 @@ +#include "pid.hpp" + +#include "math_foc.hpp" + +namespace pid { + +// PID getter functions (marked as const) +template +float PID::get_kp() const { + return kp; +} + +template +float PID::get_ki() const { + return ki; +} + +template +float PID::get_kd() const { + return kd; +} + +// PID setter functions +template +void PID::set_kp(float kp) { + this->kp = kp; +} +template +void PID::set_ki(float ki) { + this->ki = ki; +} +template +void PID::set_kd(float kd) { + this->kd = kd; +} + +template +T PID::calculate(T actual, T setpoint) { + // Calculate the error + T error = setpoint - actual; + + // Calculate the integral term + if (clock != nullptr) { + integral += error * (clock->get_time_us() - last_time) / clock->kMicrosecondsPerSecond; + } else { + integral += error; + } + // Clamp the integral term + if (integral_windup != 0) { + math::clamp(integral, -integral_windup, integral_windup); + } + + float derivative = 0; + + // Calculate the derivative term + if (clock != nullptr) { + derivative = (error - previous_error) / clock->get_time_us(); + } else { + derivative = (error - previous_error); + } + // Store the error for the next time + previous_error = error; + if (clock != nullptr) { + last_time = clock->get_time_us(); + } + + T output = error * kp + integral * ki + derivative * kd; + + // Clamp the output + if ((max_output != 0) || (min_output != 0)) { + math::clamp(output, min_output, max_output); + } + return output; +} + +// Reset the PID controller +template +void PID::reset() { + integral = 0; + previous_error = 0; +} + +// Register a clock object to be used for timing +template +void PID::register_clock(basilisk_hal::HAL_CLOCK* clock) { + this->clock = clock; +} + +// Set the maximum and minimum output values +template +void PID::set_max_output(T max_output) { + this->max_output = max_output; +} + +template +void PID::set_min_output(T min_output) { + this->min_output = min_output; +} + +// Set the integral windup value +template +void PID::set_integral_windup(float integral_windup) { + this->integral_windup = integral_windup; +} + +template class PID; +template class PID; + +} // namespace pid \ No newline at end of file diff --git a/src/stepper_control_loop.cpp b/src/stepper_control_loop.cpp new file mode 100644 index 0000000..7f88891 --- /dev/null +++ b/src/stepper_control_loop.cpp @@ -0,0 +1,86 @@ +#include "stepper_control_loop.hpp" + +#include "math.h" +#include "param_service.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 (param_service::ParamServer::getInstance().compile_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 (param_service::ParamServer::getInstance().compile_params.stepper_motor_disable_current_pid) { + // Set the current setpoints of the motors + motor_a_.run(param_service::ParamServer::getInstance().compile_params.stepper_motor_current_to_pwm_duty_cycle_slope * + current_scalars.first); + motor_b_.run(param_service::ParamServer::getInstance().compile_params.stepper_motor_current_to_pwm_duty_cycle_slope * + current_scalars.second); + } +} + +// 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; +} + +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; + } +} + +std::pair StepperControlLoop::determineCurrentSetpointScalars(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); +} + +} // namespace control_loop \ No newline at end of file diff --git a/src/system_manager.cpp b/src/system_manager.cpp new file mode 100644 index 0000000..3431ef9 --- /dev/null +++ b/src/system_manager.cpp @@ -0,0 +1,134 @@ +#include "system_manager.hpp" + +#include "hal_common.hpp" +#include "input_manager.hpp" +#include "param_service.hpp" + +namespace system_manager { +SystemManager::SystemManager() {} + +bool SystemManager::isOverTemp(float deg_c) const { + // get compile threshold from param service + float overtemperature_threshold_deg_c = + param_service::ParamServer::getInstance().compile_params.overtemperature_threshold_deg_c; + return deg_c >= overtemperature_threshold_deg_c; +} + +bool SystemManager::isOverVoltage(float voltage) const { + // get compile threshold from param service + float overvoltage_threshold_v = param_service::ParamServer::getInstance().compile_params.max_permissable_bus_voltage; + return voltage >= overvoltage_threshold_v; +} + +bool SystemManager::isOverCurrent(float current) const { + // get compile threshold from param service + float overcurrent_threshold_a = param_service::ParamServer::getInstance().compile_params.max_permissable_motor_current_amps; + return current >= overcurrent_threshold_a; +} + +bool SystemManager::isEnabled() const { return controller_enabled; } + +void SystemManager::enableController() { controller_enabled = true; } +void SystemManager::disableController() { controller_enabled = false; } +void SystemManager::setDesiredSpeed(float speed) { desired_speed = speed; } +void SystemManager::pollFaults() { + // get the current temperature + float temperature_deg_c = 0; + app_hal_status_E status = input_manager::InputManager::getInstance().get_board_temperature(temperature_deg_c); + if (status != APP_HAL_OK) { + return; + } + // get the current voltage + float voltage_v = 0; + status = input_manager::InputManager::getInstance().get_bus_voltage(voltage_v); + if (status != APP_HAL_OK) { + return; + } + // get the current current + float current_a = 0; + status = input_manager::InputManager::getInstance().get_motor_current(current_a); + if (status != APP_HAL_OK) { + return; + } + // check if any of the faults are active + is_fault_active = isOverTemp(temperature_deg_c) || isOverVoltage(voltage_v) || isOverCurrent(current_a); +} + +SystemManager::SystemManagerState SystemManager::getDesiredState() { + // Switch case tree to determine the desired state + switch (state) { + case SystemManagerState::DISABLED: + if (is_fault_active) { + state = SystemManagerState::FAULT; + } else if (isEnabled()) { + state = SystemManagerState::ENABLED; + } + break; + case SystemManagerState::ENABLED: + if (is_fault_active) { + state = SystemManagerState::FAULT; + } else if (!isEnabled()) { + state = SystemManagerState::DISABLED; + } + break; + case SystemManagerState::FAULT: + if (!is_fault_active) { + state = SystemManagerState::DISABLED; + } + break; + default: + break; + } + return state; +} + +float SystemManager::getSpeedToRun() const { return commanded_speed; } + +void SystemManager::runState(SystemManager::SystemManagerState state) { + switch (state) { + case SystemManagerState::DISABLED: + commanded_speed = 0.0f; + break; + case SystemManagerState::ENABLED: + commanded_speed = desired_speed; + break; + case SystemManagerState::FAULT: + disableController(); + commanded_speed = 0.0f; + break; + default: + break; + } +} + +void SystemManager::receive_set_speed_msg(comm::generic_message_header_t header, const void* message) { + // Cast the message to a SetSpeedMessage + comm::SetSpeedMessage set_speed_message; + set_speed_message.populateDataFromBuffer((const uint8_t*)message, header.message_length); + + // Check that the channel ID is correct + if (header.message_channel != (comm::channel_id_t)comm::ChannelID::SET_SPEED) { + return; + } + + // Check that the message length is correct + if (header.message_length != sizeof(comm::SetSpeedMessage::set_speed_message_t)) { + return; + } + + // Set the desired speed + setDesiredSpeed(set_speed_message.data.speed); +} + +void SystemManager::receive_set_speed_msg_wrapper(comm::generic_message_header_t header, const void* message, void* context) { + (void)context; + SystemManager::getInstance().receive_set_speed_msg(header, message); +} + +void SystemManager::run() { + pollFaults(); + SystemManagerState desired_state = getDesiredState(); + runState(desired_state); +} + +} // namespace system_manager diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..dc331a6 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.8) + +set(This tests) + +# set the test sources to everything in this folder that is cpp +file(GLOB SOURCES "*.cpp") +# add the main project sources in ../src to the test sources +file(GLOB MAIN_SOURCES "../src/*.cpp") + +# include the headers from the main project, which is located at "../inc/" +include_directories(../inc) +include_directories((../test/)) + +add_test( + NAME ${This} + COMMAND ${This} +) + +# add compiler definition for UNIT_TEST +add_definitions(-DUNIT_TEST) + +add_executable(${This} ${SOURCES} ${MAIN_SOURCES}) +target_link_libraries(${This} PUBLIC gtest_main gtest gmock) diff --git a/test/brushed_control_loop_test.cpp b/test/brushed_control_loop_test.cpp new file mode 100644 index 0000000..a62952f --- /dev/null +++ b/test/brushed_control_loop_test.cpp @@ -0,0 +1,117 @@ +#ifndef BRUSHED_CONTROL_LOOP_TEST_HPP +#define BRUSHED_CONTROL_LOOP_TEST_HPP +#include "brushed_control_loop.hpp" + +#include "gmock/gmock.h" +#include "param_service.hpp" + +namespace control_loop { + +using namespace ::testing; + +class BrushedControlLoopTest { + public: + BrushedControlLoopTest() : brushed_control_loop_() {} + + // Define a getter method for the brushed_control_loop_ member variable that returns a reference + BrushedControlLoop& get_brushed_control_loop() { return brushed_control_loop_; } + + // 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); + } + + utime_t& get_last_speed_dir_change_time_us() { return brushed_control_loop_.last_speed_dir_change_time_us_; } + + private: + BrushedControlLoop brushed_control_loop_; +}; + +// 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; + BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( + 0.0f, false, (utime_t)param_service::ParamServer::getInstance().compile_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); +} + +// 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; + BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( + 0.0f, true, (utime_t)param_service::ParamServer::getInstance().compile_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); +} + +// 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; + BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( + control_loop::ControlLoop::MAX_MOTOR_SPEED, false, + (utime_t)param_service::ParamServer::getInstance().compile_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); +} + +// 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; + BrushedControlLoop::h_bridge_motor_speed_outputs_t motor_speed_outputs = brushed_control_loop.compute_motor_speed_outputs( + -control_loop::ControlLoop::MAX_MOTOR_SPEED, false, + (utime_t)param_service::ParamServer::getInstance().compile_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); +} + +// 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; + 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, + (utime_t)param_service::ParamServer::getInstance().compile_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); +} + +// 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; + 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, + param_service::ParamServer::getInstance().compile_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); +} + +} // namespace control_loop + +#endif // BRUSHED_CONTROL_LOOP_TEST_HPP \ No newline at end of file diff --git a/test/brushless_6step_control_loop_test.cpp b/test/brushless_6step_control_loop_test.cpp new file mode 100644 index 0000000..41ff0b5 --- /dev/null +++ b/test/brushless_6step_control_loop_test.cpp @@ -0,0 +1,224 @@ +#include "brushless_6step_control_loop.hpp" + +#include "mock_bridge_3phase.hpp" +#include "mock_hal_adc.hpp" +#include "mock_hal_clock.hpp" +#include "param_service.hpp" + +namespace control_loop { +using namespace ::testing; + +class Brushless6StepControlLoopTest { + public: + // Define a mock HAL clock object + basilisk_hal::MOCK_HAL_CLOCK mock_clock_; + + // Define a mock 3 phase bridge + hwbridge::MOCK_BRIDGE_3PHASE mock_bridge_3phase_; + + // Define the control loop object + Brushless6StepControlLoop brushless_6step_control_loop_; + + Brushless6StepControlLoopTest(hwbridge::BldcRotorSectorSensor* rotor_sensor = nullptr) + : mock_clock_(), mock_bridge_3phase_(), brushless_6step_control_loop_(mock_bridge_3phase_, mock_clock_, rotor_sensor) {} + + // get desired state + Brushless6StepControlLoop::Brushless6StepControlLoopState get_desired_state(utime_t current_time_us, utime_t time_at_start, + float motor_speed) { + return brushless_6step_control_loop_.get_desired_state(current_time_us, time_at_start, motor_speed); + } + + // get zero crossing detected + bool get_zero_crossing_detected(const hwbridge::Bridge3Phase::bemf_voltage_t& bemf_voltage, uint8_t commutation_step) { + return brushless_6step_control_loop_.zero_crossing_detected(bemf_voltage, commutation_step); + } + + void generate_commutation_duty_cycles(hwbridge::Bridge3Phase::phase_command_t phase_command[3], uint8_t commutation_step, + float motor_speed) { + brushless_6step_control_loop_.generate_commutation_duty_cycles(phase_command, commutation_step, motor_speed); + } + + utime_t calculate_average_commutation_step_delta_time(utime_t new_commutation_step_delta_time, + utime_t* average_commutation_step_delta_time_array, size_t size) { + return brushless_6step_control_loop_.calculate_average_commutation_step_delta_time( + new_commutation_step_delta_time, average_commutation_step_delta_time_array, size); + } +}; + +// Brushless 6step control loop state machine test +// Test the get desired state function when in deadband returns stop instead of startup +TEST(Brushless6StepControlLoopTest, get_desired_state_deadband) { + Brushless6StepControlLoopTest test; + + // Set the current time to 0 + utime_t current_time_us = 0; + + // Set the time since start to 0 + utime_t time_at_start = 0; + + // Set the motor speed to half of the deadband + float motor_speed = param_service::ParamServer::getInstance().compile_params.sensorless_speed_deadband_scale * 0.5f; + + // Get the desired state + Brushless6StepControlLoop::Brushless6StepControlLoopState desired_state = + test.get_desired_state(current_time_us, time_at_start, motor_speed); + + // Check that the desired state is stop + EXPECT_EQ(desired_state, Brushless6StepControlLoop::Brushless6StepControlLoopState::STOP); +} + +// Test the get desired state function when commanded with something outside the deadband returns startup +TEST(Brushless6StepControlLoopTest, get_desired_state_startup) { + Brushless6StepControlLoopTest test; + + // Set the current time to 1 + utime_t current_time_us = 1; + + // Set the time since start to 0 + utime_t time_at_start = 0; + + // Set the motor speed to half of the deadband + float motor_speed = param_service::ParamServer::getInstance().compile_params.sensorless_speed_deadband_scale * 1.5f; + + // Get the desired state + Brushless6StepControlLoop::Brushless6StepControlLoopState desired_state = + test.get_desired_state(current_time_us, time_at_start, motor_speed); + + // Check that the desired state is start + EXPECT_EQ(desired_state, Brushless6StepControlLoop::Brushless6StepControlLoopState::START); +} + +// Test the get desired state function, after the timeout time while in startup and commanded with a value outside the deadband, +// returns run +TEST(Brushless6StepControlLoopTest, get_desired_state_run) { + Brushless6StepControlLoopTest test; + + // Set the current time to 1 + utime_t current_time_us = + param_service::ParamServer::getInstance().compile_params.sensorless_phase_motor_startup_sequence_time_us - 1; + + // Set the time since start to 0 + utime_t time_at_start = 0; + + // Set the motor speed to half of the deadband + float motor_speed = param_service::ParamServer::getInstance().compile_params.sensorless_speed_deadband_scale * 1.5f; + + // Get the desired state + Brushless6StepControlLoop::Brushless6StepControlLoopState desired_state = + test.get_desired_state(current_time_us, time_at_start, motor_speed); + + // Check that the desired state is start + EXPECT_EQ(desired_state, Brushless6StepControlLoop::Brushless6StepControlLoopState::START); + current_time_us = param_service::ParamServer::getInstance().compile_params.sensorless_phase_motor_startup_sequence_time_us; + desired_state = test.get_desired_state(current_time_us, time_at_start, motor_speed); + EXPECT_EQ(desired_state, Brushless6StepControlLoop::Brushless6StepControlLoopState::RUN); +} + +// Test the zero crossing detection function +TEST(Brushless6StepControlLoopTest, zero_crossing_detection) { + Brushless6StepControlLoopTest test; + const uint8_t sector = 4; + + // Bemf voltage struct + hwbridge::Bridge3Phase::bemf_voltage_t bemf_voltage; + bemf_voltage.u = 3.3f; + bemf_voltage.v = 0.0f; + bemf_voltage.w = 3.0f; + + // Check that the zero crossing is not detected + EXPECT_FALSE(test.get_zero_crossing_detected(bemf_voltage, sector)); + + // Check that the zero crossing is detected + bemf_voltage.u = 3.3f; + bemf_voltage.v = 0.0f; + bemf_voltage.w = 1.0f; + EXPECT_TRUE(test.get_zero_crossing_detected(bemf_voltage, sector)); +} + +// Test the generate commutation duty cycle function +TEST(Brushless6StepControlLoopTest, generate_commutation_duty_cycle) { + Brushless6StepControlLoopTest test; + + // Phase command struct + hwbridge::Bridge3Phase::phase_command_t phase_command[3]; + + // Check that the phase command is correct for a motor speed of 1 + // Comm step U low, V Z, W high + test.generate_commutation_duty_cycles(phase_command, 2, 1.0f); + EXPECT_FLOAT_EQ(phase_command[0].duty_cycle_high_side, 0.0f); + EXPECT_FLOAT_EQ(phase_command[0].invert_low_side, true); + EXPECT_FLOAT_EQ(phase_command[1].duty_cycle_high_side, 0.0f); + EXPECT_FLOAT_EQ(phase_command[1].invert_low_side, false); + EXPECT_FLOAT_EQ(phase_command[2].duty_cycle_high_side, 1.0f); + EXPECT_FLOAT_EQ(phase_command[2].invert_low_side, true); +} + +// Test the average commutation step delta time function +TEST(Brushless6StepControlLoopTest, average_commutation_step_delta_time) { + Brushless6StepControlLoopTest test; + + // Array of commutation step delta times + utime_t commutation_step_delta_time_array[3] = {1000, 2000, 3000}; + + // Check that the average commutation step delta time is correct + EXPECT_EQ(test.calculate_average_commutation_step_delta_time(4000, commutation_step_delta_time_array, 3), 2000); +} + +// Ensure that if the average commutation step delta time array is empty, the average commutation step delta time is 0 +TEST(Brushless6StepControlLoopTest, average_commutation_step_delta_time_empty_array) { + Brushless6StepControlLoopTest test; + + // Array of commutation step delta times + utime_t commutation_step_delta_time_array[3] = {0, 0, 0}; + + // Check that the average commutation step delta time is correct + EXPECT_EQ(test.calculate_average_commutation_step_delta_time(4000, commutation_step_delta_time_array, 3), 0); +} + +// If a rotor sensor is provided, ensure that the state machine returns stop when less than the sensored deadband +TEST(Brushless6StepControlLoopTest, rotor_sensor_stop) { + // Define a rotor sensor + hwbridge::MOCK_ROTOR_SECTOR_SENSOR rotor_sensor; + Brushless6StepControlLoopTest test(&rotor_sensor); + + // Set the current time to 1 + utime_t current_time_us = 1; + + // Set the time since start to 0 + utime_t time_at_start = 0; + + // Set the motor speed to half of the deadband + float motor_speed = param_service::ParamServer::getInstance().compile_params.sensored_speed_deadband_scale * 0.5f; + + // Get the desired state + Brushless6StepControlLoop::Brushless6StepControlLoopState desired_state = + test.get_desired_state(current_time_us, time_at_start, motor_speed); + + // Check that the desired state is stop + EXPECT_EQ(desired_state, Brushless6StepControlLoop::Brushless6StepControlLoopState::STOP); +} + +// If a rotor sensor is provided, ensure that the state machine returns run when greater than the sensored deadband +TEST(Brushless6StepControlLoopTest, rotor_sensor_run) { + // Define a rotor sensor + hwbridge::MOCK_ROTOR_SECTOR_SENSOR rotor_sensor; + Brushless6StepControlLoopTest test(&rotor_sensor); + + // Set the current time to 1 + utime_t current_time_us = 1; + + // Set the time since start to 0 + utime_t time_at_start = 0; + + // Set the motor speed to half of the deadband + float motor_speed = param_service::ParamServer::getInstance().compile_params.sensored_speed_deadband_scale * 1.5f; + + // Get the desired state + Brushless6StepControlLoop::Brushless6StepControlLoopState desired_state = + test.get_desired_state(current_time_us, time_at_start, motor_speed); + + // Check that the desired state is stop + EXPECT_EQ(desired_state, Brushless6StepControlLoop::Brushless6StepControlLoopState::RUN); +} + +} // namespace control_loop \ No newline at end of file diff --git a/test/brushless_foc_control_loop_test.cpp b/test/brushless_foc_control_loop_test.cpp new file mode 100644 index 0000000..c57c679 --- /dev/null +++ b/test/brushless_foc_control_loop_test.cpp @@ -0,0 +1,187 @@ +#include "brushless_foc_control_loop.hpp" + +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "math.h" +#include "math_foc.hpp" +#include "mock_bridge_3phase.hpp" +#include "mock_hal_adc.hpp" +#include "mock_hal_clock.hpp" +#include "mock_hal_timer.hpp" +#include "param_service.hpp" + +namespace control_loop { +using namespace ::testing; + +basilisk_hal::MOCK_HAL_CLOCK mock_clock; + +class BrushlessFocControlLoopTest { + public: + // Define a mock HAL clock object + + // Define a mock 3 phase bridge + hwbridge::MOCK_BRIDGE_3PHASE mock_bridge_3phase_; + + // Define the control loop object + BrushlessFocControlLoop brushless_foc_control_loop_; + + BrushlessFocControlLoop::BrushlessFocControLoopParams test_params_{ + .kp_q_current = 0.0f, + .ki_q_current = 0.0f, + .kd_q_current = 0.0f, + .kp_d_current = 0.0f, + .ki_d_current = 0.0f, + .kd_d_current = 0.0f, + .foc_start_timeout_period_us = 1000000, + .speed_to_iq_gain = 0.0f, + .i_d_reference = 0.0f, + .open_loop_full_speed_theta_velocity = 0.0f, + .pwm_control_type = BrushlessFocControlLoop::BrushlessFocPwmControlType::SPACE_VECTOR, + }; + + BrushlessFocControlLoopTest(BldcElectricalRotorPositionEstimator& rotor_position_estimator, basilisk_hal::HAL_CLOCK& clock_) + : mock_bridge_3phase_(), brushless_foc_control_loop_(mock_bridge_3phase_, clock_, rotor_position_estimator) {} + + BrushlessFocControlLoop::BrushlessFocControlLoopState get_desired_state( + float motor_speed, const BrushlessFocControlLoop::BrushlessFocControlLoopState current_state) { + return brushless_foc_control_loop_.get_desired_state(motor_speed, current_state); + } + + void init(BrushlessFocControlLoop::BrushlessFocControLoopParams* params) { brushless_foc_control_loop_.init(params); } + + void update_state(BrushlessFocControlLoop::BrushlessFocControlLoopState state) { brushless_foc_control_loop_.state_ = state; } + + BrushlessFocControlLoop::BrushlessFocControlLoopType get_control_loop_type(bool is_estimator_valid) { + return brushless_foc_control_loop_.get_desired_control_loop_type(is_estimator_valid); + } +}; + +TEST(BrushlessFocTest, test_angle_one_for_one) { + // Create a mock rotor sensor + hwbridge::MOCK_ROTOR_SECTOR_SENSOR sector_sensor; + // Initialize a sector sensor from hall + control_loop::BldcElectricalRotorPositionEstimatorFromHall rotor_estimator(mock_clock, sector_sensor); + + // Make a param struct for the rotor estimator + control_loop::BldcElectricalRotorPositionEstimatorFromHall::BldcElectricalRotorPositionEstimatorFromHallParams_t params{ + .num_hall_updates_to_start = 10, + .max_estimate_angle_overrun = 2.0f / 3.0f * M_PI, + }; + + rotor_estimator.init(¶ms); + + // Expect a call to get the sector position and ensure the reference is updated to return 3 + EXPECT_CALL(sector_sensor, get_sector(_)) // _ allowing any param + .WillOnce(DoAll(SetArgReferee<0>(1), Return(APP_HAL_OK))); + + // Update the rotor position + rotor_estimator.update(500); + + float rotor_position = 0.0f; + rotor_estimator.get_rotor_position(rotor_position); + + EXPECT_FLOAT_EQ(rotor_position, 2.0f * 2.0f * M_PI / 6.0f); + + // Expect raw float angle to be 1.0f * 2.0f * M_PI / 6.0f + float raw_hall_angle = 0.0f; + rotor_estimator.get_raw_hall_angle(raw_hall_angle); + EXPECT_FLOAT_EQ(raw_hall_angle, 1.0f * 2.0f * M_PI / 6.0f); + + // Expect the velocity to be (PI/3 rad) / (500 us) = 2094.395 rad/s + // However, we use compensated velocity, which is 2x the velocity due to the sudden velocity jump + // this is done to avoid discontinuities in the velocity + const float actual_velocity = M_PI / (3.0f * (500.0f / mock_clock.kMicrosecondsPerSecond)); + const float compensated_velocity = 1.0f * actual_velocity; + float rotor_velocity = 0.0f; + rotor_estimator.get_rotor_velocity(rotor_velocity); + EXPECT_FLOAT_EQ(rotor_velocity, compensated_velocity); +} + +TEST(BrushlessFocTest, test_angle_underflow) { + // Create a mock rotor sensor + hwbridge::MOCK_ROTOR_SECTOR_SENSOR sector_sensor; + // Initialize a sector sensor from hall + control_loop::BldcElectricalRotorPositionEstimatorFromHall rotor_estimator(mock_clock, sector_sensor); + + control_loop::BldcElectricalRotorPositionEstimatorFromHall::BldcElectricalRotorPositionEstimatorFromHallParams_t params{ + .num_hall_updates_to_start = 10, + .max_estimate_angle_overrun = 2.0f / 3.0f * M_PI, + }; + + rotor_estimator.init(¶ms); + + // Expect a call to get the sector position and ensure the reference is updated to return 3 + EXPECT_CALL(sector_sensor, get_sector(_)) // _ allowing any param + .WillOnce(DoAll(SetArgReferee<0>(5), Return(APP_HAL_OK))); + + // Update the rotor position + rotor_estimator.update(500); + + float rotor_position = 0.0f; + rotor_estimator.get_rotor_position(rotor_position); + + // Expect raw float angle to be 1.0f * 2.0f * M_PI / 6.0f + float raw_hall_angle = 0.0f; + rotor_estimator.get_raw_hall_angle(raw_hall_angle); + EXPECT_FLOAT_EQ(raw_hall_angle, 5.0f * 2.0f * M_PI / 6.0f); + + // Expect the velocity to be -(PI/3 rad) / (500 us) = 2094.395 rad/s + // However, we use compensated velocity, which is 2x the velocity due to the sudden velocity jump + + const float actual_velocity = -M_PI / (3.0f * (500.0f / mock_clock.kMicrosecondsPerSecond)); + const float compensated_velocity = 1.0f * actual_velocity; + float rotor_velocity = 0.0f; + rotor_estimator.get_rotor_velocity(rotor_velocity); + EXPECT_FLOAT_EQ(rotor_velocity, compensated_velocity); +} + +// Test the state machine transition from stop to start, and a time out makes it go back to stop +TEST(BrushlessFocTest, test_stop_to_start_to_run) { + // Create a mock rotor sensor + hwbridge::MOCK_ROTOR_SECTOR_SENSOR sector_sensor; + // Initialize a sector sensor from hall + control_loop::BldcElectricalRotorPositionEstimatorFromHall rotor_estimator(mock_clock, sector_sensor); + + // instantiate a brushless foc control loop test class + BrushlessFocControlLoopTest test_control_loop(rotor_estimator, mock_clock); + + // Call the desired state function with a time of 0, time at start of 0, and a motor speed of 0 + // Ensure the desired state is NOT_INITIALIZED + EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFocControlLoop::BrushlessFocControlLoopState::NOT_INITIALIZED), + BrushlessFocControlLoop::BrushlessFocControlLoopState::NOT_INITIALIZED); + + test_control_loop.init(&test_control_loop.test_params_); + + EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFocControlLoop::BrushlessFocControlLoopState::NOT_INITIALIZED), + BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP); + + // Call the desired state function with a time of 0, time at start of 0, and a motor speed of 0 + // Ensure that the desired state is stop + EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP), + BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP); + + // Call the desired state function with a time of foc_start_timeout_period -1, time at start of 0, and a motor speed of 0.1, + // with the rotor estimator valid Ensure that the desired state is run + EXPECT_EQ(test_control_loop.get_desired_state(0.1, BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP), + BrushlessFocControlLoop::BrushlessFocControlLoopState::RUN); +} + +// Test the state machine transition from run to stop +TEST(BrushlessFocTest, test_run_to_stop) { + // Create a mock rotor sensor + hwbridge::MOCK_ROTOR_SECTOR_SENSOR sector_sensor; + // Initialize a sector sensor from hall + control_loop::BldcElectricalRotorPositionEstimatorFromHall rotor_estimator(mock_clock, sector_sensor); + + // instantiate a brushless foc control loop test class + BrushlessFocControlLoopTest test_control_loop(rotor_estimator, mock_clock); + + // Call the desired state function with a time of 0, time at start of 0, and a motor speed of 0 + // Ensure that the desired state is stop + EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFocControlLoop::BrushlessFocControlLoopState::RUN), + BrushlessFocControlLoop::BrushlessFocControlLoopState::STOP); +} + +// Test the FOC phase voltage generator function + +} // namespace control_loop \ No newline at end of file diff --git a/test/comm_manager_test.cpp b/test/comm_manager_test.cpp new file mode 100644 index 0000000..44a822c --- /dev/null +++ b/test/comm_manager_test.cpp @@ -0,0 +1,141 @@ +#include "comm_manager.hpp" + +#include "comm_message_types.hpp" +#include "gtest/gtest.h" +#include "mock_hal_serial.hpp" + +using namespace ::testing; + +namespace comm { + +// Define a test for the determine_packet_metadata function +TEST(CommSerial, determine_packet_metadata) { + // Create a buffer to read from + uint8_t buffer[10] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09}; + // Create the expected header + generic_message_header_t expected_header = {0x00, 0x0102, 0x0304, 0x05}; + + // Call the function + generic_message_header_t header = CommManager::determine_packet_metadata(buffer, 10); + + // Check that the header is correct + EXPECT_EQ(header.SOF, expected_header.SOF); + EXPECT_EQ(header.message_channel, expected_header.message_channel); + EXPECT_EQ(header.message_length, expected_header.message_length); + EXPECT_EQ(header.message_sequence, expected_header.message_sequence); +} + +// Ensure that create_packet_metadata returns the correct header +TEST(CommSerial, create_packet_metadata) { + // Create the expected header + ChannelID message_channel = ChannelID::SYSTEM_MANAGER_STATE; + uint16_t message_length = 0x0304; + uint8_t sequence = 0x05; + + // Call the function + generic_message_header_t header = CommManager::create_packet_metadata(message_channel, message_length, sequence); + + // Convert it back into a buffer + uint8_t buffer[sizeof(generic_message_header_t)] = {0}; + CommManager::create_message_header_buffer(header, buffer, sizeof(buffer)); + + // Check that the data of the header is correct + uint8_t* header_data = buffer; + EXPECT_EQ(header_data[0], 0x00); + EXPECT_EQ(header_data[1], (uint16_t)message_channel >> 8); + EXPECT_EQ(header_data[2], (uint16_t)message_channel & 0xFF); + EXPECT_EQ(header_data[3], message_length >> 8); + EXPECT_EQ(header_data[4], message_length & 0xFF); +} + +// Ensure that the registerCallback function returns true +TEST(CommSerial, registerCallback) { + // Create a mock HAL_SERIAL + basilisk_hal::MockSerial mock_hal_serial; + + // Create a CommManager + CommManager comm_manager(mock_hal_serial); + + // Create a callback + void (*callback)(generic_message_header_t, const void*, void*) = [](generic_message_header_t header, const void* message, + void* context) { + (void)header; + (void)message; + (void)context; + }; + + // Register the callback + bool success = comm_manager.registerCallback(ChannelID::SYSTEM_MANAGER_STATE, callback, NULL); + + // Check that the registration was successful + EXPECT_TRUE(success); +} + +// Ensure that the sendASAP function returns true +TEST(CommSerial, sendASAP) { + // Create a mock HAL_SERIAL + basilisk_hal::MockSerial mock_hal_serial; + + // Create a CommManager + CommManager comm_manager(mock_hal_serial); + + // Create a message + ArmDisarmMessage message = ArmDisarmMessage(false); + + // Expect a call to transmit on the mock hal serial object + EXPECT_CALL(mock_hal_serial, transmit(_, _)).WillOnce(Return(APP_HAL_OK)); + + // Send the message + bool success = comm_manager.sendASAP((ChannelID)0x00, message); + + // Check that the send was successful + EXPECT_TRUE(success); +} + +static void set_speed_msg_test_callback(generic_message_header_t header, const void* message, void* context) { + (void)context; + // Cast the message to a SetSpeedMessage + SetSpeedMessage set_speed_message; + set_speed_message.populateDataFromBuffer((const uint8_t*)message, header.message_length); + + // Check that the channel ID is correct + EXPECT_EQ(header.message_channel, (channel_id_t)ChannelID::SET_SPEED); + + // Check that the message length is correct + EXPECT_EQ(header.message_length, sizeof(SetSpeedMessage::set_speed_message_t)); + + // Check that the speed is correct + EXPECT_FLOAT_EQ(set_speed_message.data.speed, 0.5); +} +// Register a callback for the set speed function on the set speed channel. Ensure that the callback is called +TEST(CommSerial, set_speed_msg_test_callback) { + // Create a mock HAL_SERIAL + basilisk_hal::MockSerial mock_hal_serial; + + // Create a CommManager + CommManager comm_manager(mock_hal_serial); + + // Register a callback for the set speed function on the set speed channel + comm_manager.registerCallback(ChannelID::SET_SPEED, set_speed_msg_test_callback, NULL); + + // Create a fake buffer of message header and data to process + uint8_t buffer[sizeof(generic_message_header_t) + sizeof(SetSpeedMessage::set_speed_message_t)]; + + // Populate the buffer with the message header + generic_message_header_t header = + CommManager::create_packet_metadata(ChannelID::SET_SPEED, sizeof(SetSpeedMessage::set_speed_message_t), 0x00); + CommManager::create_message_header_buffer(header, buffer, sizeof(buffer)); + + // Populate the buffer with the message data + SetSpeedMessage::set_speed_message_t* data = + (SetSpeedMessage::set_speed_message_t*)(buffer + sizeof(generic_message_header_t)); + data->speed = 0.5; + + // Process the buffer + comm_manager.process_received_data(buffer, sizeof(buffer)); + + // Check that the callback was called + EXPECT_TRUE(true); +} + +} // namespace comm diff --git a/test/foc_math_test.cpp b/test/foc_math_test.cpp new file mode 100644 index 0000000..992335e --- /dev/null +++ b/test/foc_math_test.cpp @@ -0,0 +1,120 @@ +#include "gtest/gtest.h" +#include "math.h" +#include "math_foc.hpp" + +namespace math { +// Test the clarke transform function +TEST(MathFOCTest, test_clarke_transform_all_current_1) { + // Test the clarke transform function + math::clarke_transform_result_t result = math::clarke_transform(1.0, 1.0, 1.0); + EXPECT_EQ(result.alpha, 1.0); + EXPECT_EQ(result.beta, 0.0); +} + +// Test it with a value of ia = 0.5 and ib = 0.75 and ic = 0.25 +TEST(MathFOCTest, test_clarke_transform_ia_0_5_ib_0_75_ic_0_25) { + // Test the clarke transform function + math::clarke_transform_result_t result = math::clarke_transform(0.5, 0.75, 0.25); + EXPECT_EQ(result.alpha, 0.5); + // Beta result + float beta_result = (0.75 - 0.25) / math::sqrt_3; + EXPECT_FLOAT_EQ(result.beta, beta_result); +} + +// Test the park transform function +TEST(MathFOCTest, test_park_transform_0_degrees) { + // Test the clarke transform function + math::park_transform_result_t result = math::park_transform(2.0, 2.0, 0.0); + EXPECT_FLOAT_EQ(result.d, 2.0); + EXPECT_FLOAT_EQ(result.q, 2.0); +} + +// Test the park transform function with 90 degrees +TEST(MathFOCTest, test_park_transform_90_degrees) { + // Test the clarke transform function + math::park_transform_result_t result = math::park_transform(2.0, 2.0, 90.0 * M_PI / 180.0); + EXPECT_FLOAT_EQ(result.d, 2.0); + EXPECT_FLOAT_EQ(result.q, -2.0); +} + +// Test the park transform function with 180 degrees +TEST(MathFOCTest, test_park_transform_180_degrees) { + // Test the clarke transform function + math::park_transform_result_t result = math::park_transform(2.0, 2.0, 180.0 * M_PI / 180.0); + EXPECT_FLOAT_EQ(result.d, -2.0); + EXPECT_FLOAT_EQ(result.q, -2.0); +} + +// Test the park transform function with 270 degrees +TEST(MathFOCTest, test_park_transform_270_degrees) { + // Test the clarke transform function + math::park_transform_result_t result = math::park_transform(2.0, 2.0, 270.0 * M_PI / 180.0); + EXPECT_FLOAT_EQ(result.d, -2.0); + EXPECT_FLOAT_EQ(result.q, 2.0); +} + +// Test the park transform function with 45 degrees +TEST(MathFOCTest, test_park_transform_45_degrees) { + // Test the clarke transform function + math::park_transform_result_t result = math::park_transform(2.0, 2.0, 45.0 * M_PI / 180.0); + float theta_rad = 45.0 * M_PI / 180.0; + float q_result = 2.0 * sin(theta_rad) - 2.0 * cos(theta_rad); + float d_result = 2.0 * sin(theta_rad) + 2.0 * cos(theta_rad); + EXPECT_FLOAT_EQ(result.d, d_result); + EXPECT_FLOAT_EQ(result.q, q_result); +} + +// Test the wraparound function +TEST(MathFocTest, test_wraparound) { + float angle = -0.4f; + + // Wrap the angle around 0 -> 1. Expect the result to be 0.6 + math::wraparound(angle, 0.0f, 1.0f); + EXPECT_FLOAT_EQ(angle, 0.6f); + + // Make the angle 1.2. Expect the result to be 0.2 + angle = 1.2f; + math::wraparound(angle, 0.0f, 1.0f); + EXPECT_FLOAT_EQ(angle, 0.2f); + + // Make the angle 0.5. Expect the result to be 0.5 + angle = 0.5f; + math::wraparound(angle, 0.0f, 1.0f); + EXPECT_FLOAT_EQ(angle, 0.5f); + + // Make the angle 0.0. Expect the result to be 0.0 + angle = 0.0f; + math::wraparound(angle, 0.0f, 1.0f); + EXPECT_FLOAT_EQ(angle, 0.0f); + + // Make the angle -46.0. Expect the result to be 0.0 + angle = -46.0f; + math::wraparound(angle, 0.0f, 1.0f); + EXPECT_FLOAT_EQ(angle, 0.0f); + + // Make the angle 45.3. Expect the result to be 0.3 + angle = 45.3001f; + math::wraparound(angle, 0.0f, 1.0f); + EXPECT_NEAR(angle, 0.3001f, 0.01); +} + +// Test the trapezoidal integral function +TEST(MathFOCTest, test_trapezoidal_integral) { + float x = 1.0f; + float x_prev = 0.0f; + float y = 1.0f; + float y_prev = 0.0f; + + float result = math::trapezoidal_integral(x, x_prev, y, y_prev); + EXPECT_FLOAT_EQ(result, 0.5f); + + // Test it now with x = 2.0f, x_prev = 1.0f, y = -1.0f, y_prev = 1.0f + x = 2.0f; + x_prev = 1.0f; + y = -1.0f; + y_prev = 1.0f; + result = math::trapezoidal_integral(x, x_prev, y, y_prev); + EXPECT_FLOAT_EQ(result, 0.0f); +} + +} // namespace math \ No newline at end of file diff --git a/test/input_manager_test.cpp b/test/input_manager_test.cpp new file mode 100644 index 0000000..8f5bee2 --- /dev/null +++ b/test/input_manager_test.cpp @@ -0,0 +1,67 @@ +#include "input_manager.hpp" + +#include "gtest/gtest.h" +#include "mock_hal_adc.hpp" +#include "param_service.hpp" + +namespace input_manager { +using namespace ::testing; +// Test that the input manager does not crash if the hal_adc is not registered +TEST(InputManagerTest, test_get_board_temperature_no_hal_adc) { + float returned_temperature = 0; + EXPECT_EQ(APP_HAL_NOT_INITIALIZED, InputManager::getInstance().get_board_temperature(returned_temperature)); +} + +TEST(InputManagerTest, test_get_board_temperature) { + // Create a mock hal_adc object + basilisk_hal::MOCK_HAL_ADC hal_adc; + // Register the mock hal_adc object with the input manager + InputManager::getInstance().register_hal_adc(&hal_adc); + float test_temperature = 0; + // Expect a call to the HAL_ADC read_adc function. Return that the app hall status is ok + // The channel call should match the compile parameter temp_sensor_adc_channel + EXPECT_CALL(hal_adc, read_adc(_, param_service::ParamServer::getInstance().compile_params.temp_sensor_adc_channel)) + .WillOnce(Return(APP_HAL_OK)); + EXPECT_EQ(APP_HAL_OK, InputManager::getInstance().get_board_temperature(test_temperature)); +} + +// Test the bus voltage function in which the bus voltage is returned correctly. Inject 3.0V into the ADC and expect 6.0V to be +// returned as a function of the compile parameters +TEST(InputManagerTest, test_get_bus_voltage) { + // Create a mock hal_adc object + basilisk_hal::MOCK_HAL_ADC hal_adc; + // Register the mock hal_adc object with the input manager + InputManager::getInstance().register_hal_adc(&hal_adc); + float test_voltage = 3.0; + float actual_voltage = test_voltage; + // Expect the test_voltage to be a function of the 2 resistors that make up the voltage divider + actual_voltage *= (param_service::ParamServer::getInstance().compile_params.bus_voltage_dividor_resistor_1_ohms + + param_service::ParamServer::getInstance().compile_params.bus_voltage_dividor_resistor_2_ohms) / + param_service::ParamServer::getInstance().compile_params.bus_voltage_dividor_resistor_2_ohms; + // Expect a call to the HAL_ADC read_adc function. Return that the app hall status is ok + // The channel call should match the compile parameter bus_voltage_adc_channel + EXPECT_CALL(hal_adc, read_adc(_, param_service::ParamServer::getInstance().compile_params.bus_voltage_adc_channel)) + .WillOnce(DoAll(SetArgReferee<0>(test_voltage), Return(APP_HAL_OK))); + EXPECT_EQ(APP_HAL_OK, InputManager::getInstance().get_bus_voltage(test_voltage)); + EXPECT_FLOAT_EQ(test_voltage, actual_voltage); +} + +// Test the current function in which the current is returned correctly. Inject 1V into the ADC and expect the correct voltage +// given the compile params +TEST(InputManagerTest, test_get_current) { + // Create a mock hal_adc object + basilisk_hal::MOCK_HAL_ADC hal_adc; + // Register the mock hal_adc object with the input manager + InputManager::getInstance().register_hal_adc(&hal_adc); + float test_voltage = 1; + float actual_current = test_voltage / param_service::ParamServer::getInstance().compile_params.current_sense_resistor_ohms / + param_service::ParamServer::getInstance().compile_params.current_sense_gain; + // Expect a call to the HAL_ADC read_adc function. Return that the app hall status is ok + // The channel call should match the compile parameter current_adc_channel + EXPECT_CALL(hal_adc, read_adc(_, param_service::ParamServer::getInstance().compile_params.motor_current_adc_channel)) + .WillOnce(DoAll(SetArgReferee<0>(test_voltage), Return(APP_HAL_OK))); + EXPECT_EQ(APP_HAL_OK, InputManager::getInstance().get_motor_current(test_voltage)); + EXPECT_FLOAT_EQ(test_voltage, actual_current); +} + +} // namespace input_manager diff --git a/test/main.cpp b/test/main.cpp new file mode 100644 index 0000000..810fef4 --- /dev/null +++ b/test/main.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/mock_bridge_3phase.hpp b/test/mock_bridge_3phase.hpp new file mode 100644 index 0000000..3b55e83 --- /dev/null +++ b/test/mock_bridge_3phase.hpp @@ -0,0 +1,27 @@ +#ifndef MOCK_BRIDGE_3PHASE_HPP +#define MOCK_BRIDGE_3PHASE_HPP + +#include "bridge_3phase.hpp" +#include "bridge_3phase_drv8323.hpp" +#include "gmock/gmock.h" + +namespace hwbridge { +// Define a mock class for the 3-phase bridge +class MOCK_BRIDGE_3PHASE : public Bridge3Phase { + public: + MOCK_METHOD(void, set_phase, (const phase_command_t& u, const phase_command_t& v, const phase_command_t& w), (override)); + MOCK_METHOD(app_hal_status_E, init, (), (override)); + MOCK_METHOD(app_hal_status_E, read_bemf, (bemf_voltage_t & bemf_voltage), (override)); + MOCK_METHOD(app_hal_status_E, read_current, (phase_current_t & current), (override)); + MOCK_METHOD(app_hal_status_E, read_bus_voltage, (float& voltage), (override)); +}; + +// Define a mock class for a 3-phase rotor sector sensor +class MOCK_ROTOR_SECTOR_SENSOR : public BldcRotorSectorSensor { + public: + MOCK_METHOD(app_hal_status_E, init, (), (override)); + MOCK_METHOD(app_hal_status_E, get_sector, (uint8_t & sector), (override)); +}; + +} // namespace hwbridge +#endif // MOCK_BRIDGE_3PHASE_HPP \ No newline at end of file diff --git a/test/mock_hal_adc.hpp b/test/mock_hal_adc.hpp new file mode 100644 index 0000000..3519258 --- /dev/null +++ b/test/mock_hal_adc.hpp @@ -0,0 +1,13 @@ +#ifndef MOCK_HAL_ADC_HPP +#define MOCK_HAL_ADC_HPP +#include "gmock/gmock.h" +#include "hal_adc.hpp" + +namespace basilisk_hal { +class MOCK_HAL_ADC : public HAL_ADC { + public: + MOCK_METHOD(app_hal_status_E, read_adc, (float& result, uint8_t channel), (override)); +}; +} // namespace basilisk_hal + +#endif \ No newline at end of file diff --git a/test/mock_hal_clock.hpp b/test/mock_hal_clock.hpp new file mode 100644 index 0000000..aad3fc7 --- /dev/null +++ b/test/mock_hal_clock.hpp @@ -0,0 +1,15 @@ +#ifndef MOCK_HAL_CLOCK_HPP +#define MOCK_HAL_CLOCK_HPP + +#include "gmock/gmock.h" +#include "hal_clock.hpp" + +namespace basilisk_hal { + +class MOCK_HAL_CLOCK : public HAL_CLOCK { + public: + MOCK_METHOD(utime_t, get_time_us, (), (override)); +}; + +} // namespace basilisk_hal +#endif \ No newline at end of file diff --git a/test/mock_hal_serial.hpp b/test/mock_hal_serial.hpp new file mode 100644 index 0000000..ed75ac1 --- /dev/null +++ b/test/mock_hal_serial.hpp @@ -0,0 +1,22 @@ +#ifndef MOCK_HAL_SERIAL_HPP +#define MOCK_HAL_SERIAL_HPP + +#include "gmock/gmock.h" +#include "hal_serial.hpp" + +namespace basilisk_hal { + +class MockSerial : public HAL_SERIAL { + public: + MockSerial() = default; + ~MockSerial() override = default; + + MOCK_METHOD(app_hal_status_E, transmit, (const void* byte, size_t size), (override)); + MOCK_METHOD(app_hal_status_E, receive, (void* byte, size_t& size), (override)); + MOCK_METHOD(app_hal_status_E, async_register_callback, + (void (*callback)(const uint8_t*, const size_t, void*), const void* context), (override)); +}; + +} // namespace basilisk_hal + +#endif // MOCK_HAL_SERIAL_HPP \ No newline at end of file diff --git a/test/mock_hal_timer.hpp b/test/mock_hal_timer.hpp new file mode 100644 index 0000000..cc393f1 --- /dev/null +++ b/test/mock_hal_timer.hpp @@ -0,0 +1,38 @@ +#ifndef MOCK_HAL_TIMER_HPP +#define MOCK_HAL_TIMER_HPP + +#include "gmock/gmock.h" +#include "hal_timer.hpp" + +namespace basilisk_hal { + +class MOCK_HAL_TIMER : public HAL_Timer { + public: + MOCK_METHOD(void, set_timer_params, (timer_mode_E mode, uint32_t frequency), (override)); + MOCK_METHOD(void, start, (), (override)); + MOCK_METHOD(void, stop, (), (override)); + MOCK_METHOD(void, set_channel, (uint32_t ticks, timer_channel_E channel), (override)); + MOCK_METHOD(void, set_period, (uint32_t period), (override)); + MOCK_METHOD(void, reset, (), (override)); + MOCK_METHOD(uint32_t, get_period, (), (override)); +}; + +// Create a complementary PWM timer class +class MOCK_HAL_ComplementaryPWM_Timer : public HAL_ComplementaryPWM_Timer { + public: + MOCK_METHOD(void, set_timer_params, (timer_mode_E mode, uint32_t frequency), (override)); + MOCK_METHOD(void, start, (), (override)); + MOCK_METHOD(void, stop, (), (override)); + MOCK_METHOD(void, set_channel, (uint32_t ticks, timer_channel_E channel), (override)); + MOCK_METHOD(void, set_period, (uint32_t period), (override)); + MOCK_METHOD(void, reset, (), (override)); + MOCK_METHOD(uint32_t, get_period, (), (override)); + MOCK_METHOD(void, set_channel_with_complementary_phase, + (uint32_t ticks, timer_channel_E channel, + HAL_ComplementaryPWM_Timer::complementary_PWM_phase_E complementary_channel_phase), + (override)); +}; + +} // namespace basilisk_hal + +#endif \ No newline at end of file diff --git a/test/param_test.cpp b/test/param_test.cpp new file mode 100644 index 0000000..03b60aa --- /dev/null +++ b/test/param_test.cpp @@ -0,0 +1,7 @@ +#include "gtest/gtest.h" +#include "param_service.hpp" + +TEST(ParamTest, test_param_service_is_singleton) { + // test that the pointer of 2 instances are identical + EXPECT_EQ(¶m_service::ParamServer::getInstance(), ¶m_service::ParamServer::getInstance()); +} \ No newline at end of file diff --git a/test/pid_test.cpp b/test/pid_test.cpp new file mode 100644 index 0000000..c6e11e5 --- /dev/null +++ b/test/pid_test.cpp @@ -0,0 +1,73 @@ +#include "pid.hpp" + +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace pid { +// Test that setting the gains works and the same gains are returned +TEST(PIDTest, SetGainsTest) { + PID pid(1, 2, 3, -10, 10, 0); + EXPECT_EQ(pid.get_kp(), 1); + EXPECT_EQ(pid.get_ki(), 2); + EXPECT_EQ(pid.get_kd(), 3); +} + +// Test that the control value proportional gain is calculated correctly +TEST(PIDTest, ProportionalGainTest) { + PID pid(1, 0, 0, -10, 10, 0); + EXPECT_EQ(pid.get_kp(), 1.0); + EXPECT_EQ(pid.calculate(0, 1.0), 1); + EXPECT_EQ(pid.calculate(1, 0), -1); + EXPECT_EQ(pid.calculate(1, 1), 0); +} + +// Test the integral gain +TEST(PIDTest, IntegralGainTest) { + PID pid(0, 1, 0, -10, 10, 10); + EXPECT_EQ(pid.get_ki(), 1.0); + EXPECT_EQ(pid.calculate(1.0, 0.0), -1); + EXPECT_EQ(pid.calculate(0, 1), 0); + EXPECT_EQ(pid.calculate(0, 1), 1); +} + +// Test the derivative gain +TEST(PIDTest, DerivativeGainTest) { + PID pid(0, 0, 1, -10, 10, 0); + EXPECT_EQ(pid.get_kd(), 1.0); + EXPECT_EQ(pid.calculate(1.0, 0.0), -1); + EXPECT_EQ(pid.calculate(0, 1), 2); + EXPECT_EQ(pid.calculate(0, 1), 0); + EXPECT_EQ(pid.calculate(0, 5), 4); +} + +// Test the integral windup +TEST(PIDTest, IntegralWindupTest) { + PID pid(0, 1, 0, -10, 50, 10); + EXPECT_EQ(pid.get_ki(), 1.0); + EXPECT_EQ(pid.calculate(1.0, 0.0), -1); + EXPECT_EQ(pid.calculate(0, 1), 0); + EXPECT_EQ(pid.calculate(0, 1), 1); + EXPECT_EQ(pid.calculate(0, 1), 2); + EXPECT_EQ(pid.calculate(0, 1), 3); + EXPECT_EQ(pid.calculate(0, 1), 4); + EXPECT_EQ(pid.calculate(0, 1), 5); + EXPECT_EQ(pid.calculate(0, 1), 6); + EXPECT_EQ(pid.calculate(0, 1), 7); + EXPECT_EQ(pid.calculate(0, 1), 8); + EXPECT_EQ(pid.calculate(0, 1), 9); + EXPECT_EQ(pid.calculate(0, 1), 10); + EXPECT_EQ(pid.calculate(0, 1), 10); +} + +// Test the max output +TEST(PIDTest, MaxOutputTest) { + PID pid(1, 0, 0, -10, 10, 0); + EXPECT_EQ(pid.calculate(0.0, 1000.0), 10); +} + +// Test the min output +TEST(PIDTest, MinOutputTest) { + PID pid(1, 0, 0, -10, 10, 0); + EXPECT_EQ(pid.calculate(0.0, -1000.0), -10); +} +} // namespace pid \ No newline at end of file diff --git a/test/stepper_motor_test.cpp b/test/stepper_motor_test.cpp new file mode 100644 index 0000000..aeb5517 --- /dev/null +++ b/test/stepper_motor_test.cpp @@ -0,0 +1,103 @@ +#include "bridge_hbridge_drv8801.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "mock_hal_clock.hpp" +#include "stepper_control_loop.hpp" + +namespace control_loop { +// Test the determineElectricalAngle function. For now, this will use simple 4 block commutation + +// Assert that a desired speed of 0 returns the same previous angle +TEST(StepperControlLoop, determineElectricalAngleZeroSpeed) { + // 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.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); +} + +} // namespace control_loop \ No newline at end of file diff --git a/test/system_manager_test.cpp b/test/system_manager_test.cpp new file mode 100644 index 0000000..73a0a54 --- /dev/null +++ b/test/system_manager_test.cpp @@ -0,0 +1,91 @@ +#include "system_manager.hpp" + +#include "gtest/gtest.h" +#include "param_service.hpp" + +namespace system_manager { + +TEST(SystemManagerTest, test_system_manager_is_singleton) { + // test that the pointer of 2 instances are identical + EXPECT_EQ(&SystemManager::getInstance(), &SystemManager::getInstance()); +} + +TEST(SystemManagerTest, test_system_manager_is_overtemperature) { + // Test that the over temp fault is triggered when the temperature is above the threshold + // get compile threshold from param service + float overtemperature_threshold_deg_c = + param_service::ParamServer::getInstance().compile_params.overtemperature_threshold_deg_c; + EXPECT_TRUE(SystemManager::getInstance().isOverTemp(overtemperature_threshold_deg_c)); + EXPECT_FALSE(SystemManager::getInstance().isOverTemp(overtemperature_threshold_deg_c - 0.1)); +} + +TEST(SystemManagerTest, test_system_manager_is_overvoltage) { + // Test that the over voltage fault is triggered when the voltage is above the threshold + // get compile threshold from param service + float overvoltage_threshold_v = param_service::ParamServer::getInstance().compile_params.max_permissable_bus_voltage; + EXPECT_TRUE(SystemManager::getInstance().isOverVoltage(overvoltage_threshold_v)); + EXPECT_FALSE(SystemManager::getInstance().isOverVoltage(overvoltage_threshold_v - 0.1)); +} + +TEST(SystemManagerTest, test_system_manager_is_overcurrent) { + // Test that the over current fault is triggered when the current is above the threshold + // get compile threshold from param service + float overcurrent_threshold_a = param_service::ParamServer::getInstance().compile_params.max_permissable_motor_current_amps; + EXPECT_TRUE(SystemManager::getInstance().isOverCurrent(overcurrent_threshold_a)); + EXPECT_FALSE(SystemManager::getInstance().isOverCurrent(overcurrent_threshold_a - 0.1)); +} + +// Test that the system manager state machine starts in idle +TEST(SystemManagerTest, test_system_manager_state_machine_starts_in_idle) { + EXPECT_EQ(SystemManager::getInstance().getDesiredState(), SystemManager::SystemManagerState::DISABLED); + // Also test that the system manager is diasabled + EXPECT_FALSE(SystemManager::getInstance().isEnabled()); +} + +// Test that if the controller is enabled and no faults are active, the system manager state machine goes to ENABLED +TEST(SystemManagerTest, test_system_manager_state_machine_enabled) { + // Confirm we are in disabled state + EXPECT_EQ(SystemManager::getInstance().getDesiredState(), SystemManager::SystemManagerState::DISABLED); + // set that there are no active faults + SystemManager::getInstance().is_fault_active = false; + // Enable the controller + SystemManager::getInstance().enableController(); + // Check that the state machine is in ENABLED + EXPECT_EQ(SystemManager::getInstance().getDesiredState(), SystemManager::SystemManagerState::ENABLED); + // Also test that the system manager is enabled + EXPECT_TRUE(SystemManager::getInstance().isEnabled()); +} + +// Test that a fault moves the system manager state machine to FAULT +TEST(SystemManagerTest, test_system_manager_state_machine_fault) { + // set that there are active faults + SystemManager::getInstance().is_fault_active = true; + // Enable the controller + SystemManager::getInstance().enableController(); + // Check that the state machine is in FAULT + EXPECT_EQ(SystemManager::getInstance().getDesiredState(), SystemManager::SystemManagerState::FAULT); +} + +TEST(SystemManagerTest, test_system_manager_disabled_state) { + // Run the system manager + SystemManager::getInstance().runState(SystemManager::SystemManagerState::DISABLED); + // Check that the desired speed is 0 + EXPECT_EQ(SystemManager::getInstance().getSpeedToRun(), 0); + + // Run the system manager + SystemManager::getInstance().runState(SystemManager::SystemManagerState::FAULT); + // Check that the desired speed is 0 + EXPECT_EQ(SystemManager::getInstance().getSpeedToRun(), 0); +} + +// Test that the desired speed is given when running the system manager in ENABLED +TEST(SystemManagerTest, test_system_manager_enabled_state) { + // Set the desired speed + SystemManager::getInstance().setDesiredSpeed(30.5); + // Run the system manager + SystemManager::getInstance().runState(SystemManager::SystemManagerState::ENABLED); + // Check that the desired speed is 1 + EXPECT_EQ(SystemManager::getInstance().getSpeedToRun(), 30.5); +} + +} // namespace system_manager \ No newline at end of file diff --git a/test/test.cpp b/test/test.cpp new file mode 100644 index 0000000..2f207c4 --- /dev/null +++ b/test/test.cpp @@ -0,0 +1,4 @@ +#include "example.hpp" +#include "gtest/gtest.h" + +TEST(ExampleTest, test_add) { EXPECT_EQ(2, test_add_func(1, 1)); } \ No newline at end of file