diff --git a/lib/interfaces/src/MCUInterface.cpp b/lib/interfaces/src/MCUInterface.cpp index dabb3bc37..18464dc6f 100644 --- a/lib/interfaces/src/MCUInterface.cpp +++ b/lib/interfaces/src/MCUInterface.cpp @@ -212,9 +212,9 @@ void MCUInterface::tick(int fsm_state, update_mcu_status_CAN_drivetrain(inv_has_error); update_mcu_status_CAN_safety(software_is_ok); - auto drive_mode = static_cast(tc_mux_status.current_controller_mode); - auto torque_mode = static_cast(tc_mux_status.current_torque_limit_enum); - auto max_torque = tc_mux_status.current_torque_limit_value; + auto drive_mode = static_cast(tc_mux_status.active_controller_mode); + auto torque_mode = static_cast(tc_mux_status.active_torque_limit_enum); + auto max_torque = tc_mux_status.active_torque_limit_value; update_mcu_status_CAN_TCMux(drive_mode, torque_mode, max_torque); update_mcu_status_CAN_buzzer(buzzer_is_on); diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 2e399a6df..e8978c772 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -5,6 +5,7 @@ #include "SysClock.h" #include "SharedFirmwareTypes.h" +/// @brief Defines modes of torque limit to be processed in torque limit map for exact values. enum class TorqueLimit_e { TCMUX_FULL_TORQUE = 0, @@ -49,17 +50,25 @@ struct DrivetrainDynamicReport_s float measuredTorqueCurrents[NUM_MOTORS]; float measuredMagnetizingCurrents[NUM_MOTORS]; }; +/// @brief Stores setpoints for a command to the Drivetrain, containing speed and torque setpoints for each motor. These setpoints are defined in the torque controllers cycled by the TC Muxer. +/// The Speeds unit is rpm and are the targeted speeds for each wheel of the car. +/// The torques unit is nm and is the max torque requested from the inverter to reach such speeds. +/// One can use the arrays with FR(Front Left), FL(Front Left), RL(Rear Left), RR(Rear Right) to access or modify the respective set points. eg. speeds_rpm[FR] = 0.0; +/// Their indexes are defined in utility.h as follows: FL = 0, FR = 1, RL = 2, RR = 3. struct DrivetrainCommand_s { float speeds_rpm[NUM_MOTORS]; - float torqueSetpoints[NUM_MOTORS]; // FIXME: misnomer. This represents the magnitude of the torque the inverter can command to reach the commanded speed setpoint + float inverter_torque_limit[NUM_MOTORS]; }; +/// @brief Packages drivetrain command with ready boolean to give feedback on controller successfully evaluating +/// @note returned by all car controllers evaluate method struct TorqueControllerOutput_s { DrivetrainCommand_s command; bool ready; }; + struct VectornavData_s { float velocity_x; @@ -80,6 +89,7 @@ struct VectornavData_s xyz_vec angular_rates; }; +/// @brief Defines errors for TC Mux to use to maintain system safety enum class TorqueControllerMuxError { NO_ERROR = 0, @@ -89,12 +99,13 @@ enum class TorqueControllerMuxError ERROR_CONTROLLER_NULL_POINTER =4 }; +/// @brief packages TC Mux indicators: errors, mode, torque limit, bypass struct TorqueControllerMuxStatus { - TorqueControllerMuxError current_error; - ControllerMode_e current_controller_mode; - TorqueLimit_e current_torque_limit_enum; - float current_torque_limit_value; + TorqueControllerMuxError active_error; + ControllerMode_e active_controller_mode; + TorqueLimit_e active_torque_limit_enum; + float active_torque_limit_value; bool output_is_bypassing_limits; }; diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index a94556fb9..b532407bc 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -1,16 +1,26 @@ #ifndef BASECONTROLLER #define BASECONTROLLER #include "SharedDataTypes.h" + +/// @brief defines namespace for definition of a drivetrain command with no torque for clearer code in the Muxer +/// This can be used to define other specific car states in the future namespace BaseControllerParams { const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, - .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}}; + .inverter_torque_limit = {0.0, 0.0, 0.0, 0.0}}; } + /// @brief Base class for all controllers, which define drivetrain command containing different variations of +/// speed set points and necessary torque set points to be requested from the motors in order to achieve said speeds. + /// required method(s): evaluate class Controller { public: + /// @brief This mehod must be implemented by every controller in the Tc Muxer. This is called in the Muxer whenever the drivetrain command is obtained. + /// @ref TorqueControllerMux.cpp to see that in every tick of the system, the active controller must be ticked through this method + /// @param state with all sensor information to properly define torque set points + /// @return TorqueControllerOutput_s This is a Drivetrain command passed along with a state boolean to ensure car controllers are working properly virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; }; diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 0a3fbd4a4..f971a39ea 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -4,6 +4,15 @@ #include "BaseController.h" #include #include + +/// @brief Modes to define launch behavior, where each one waits for acceleration request threshold to move to next mode +/// LAUNCH_NOT_READY keeps speed at 0 and makes sure pedals are not pressed, the launch controller begins in this state +/// From this state the launch can only progress forwards to LAUNCH_READY +/// LAUNCH_READY keeps speed at 0, below the speed threshold(5 m/s) and makes sure brake is not pressed harder than the threshold of .2(20% pushed) +/// From this state the launch can progress forwards to LAUNCHING according to the two conditions defined above or backwards to LAUNCH_NOT_READY if those conditions are not met +/// LAUNCHING uses respective algorithm to set speed set point and requests torque from motors to reach it +/// From this state the launch can fully begin and set speed set points above 0.0 m/s and the maximum available torque can be requested from the inverters +/// This launch state can be terminated if the brake is pressed above the threshold(.2(20% pushed)) or if the accelerator is not pressed enough (<= .5(50% pushed)) enum class LaunchStates_e { NO_LAUNCH_MODE, @@ -12,6 +21,7 @@ enum class LaunchStates_e LAUNCHING }; +/// @brief contains constants for tick behavior/progression(_threshold variables used to determine when to move to the next step) and defaults(DEFAULT_LAUNCH_SPEED_TARGET) namespace BaseLaunchControllerParams { const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; @@ -37,6 +47,9 @@ class BaseLaunchController : public virtual Controller int16_t init_speed_target_ = 0; public: + /// @brief Constructor for parent launch controller + /// @param initial_speed_target used only in simple launch controller algorithm + /// @note requires one method: calc_launch_algo BaseLaunchController(int16_t initial_speed_target) : init_speed_target_(initial_speed_target) { @@ -44,12 +57,25 @@ class BaseLaunchController : public virtual Controller writeout_.ready = true; } + /// @brief ticks launch controller to progress through launch states when conditions are met. The conditions are explained above the launch states enum. + /// all launch controllers use this class' implementation of tick. + /// tick conatains the current system tick controlled by main.cpp + /// pedalsData conatins the brake and accelerator values + /// wheel_rpms[] contains how fast each wheel is spinning, order of wheels in this array is defined in SharedDataTypes.h and Utility.h + /// vn_data contains vector states of the car that will be provided to the calc_launch_algo method called in the LAUNCHING state of this set of modes void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], const VectornavData_s &vn_data); LaunchStates_e get_launch_state() { return launch_state_; } + /// @brief calculates how speed target (the speed the car is trying to achieve during launch) is set and/or increased during launch + /// This updates internal speed target variable launch_speed_target_ + /// @param vn_data vector data needed for calulations eg. speed and coordinates + /// @note defines important variation in launch controller tick/evaluation as the launch controllers share a tick method defined in this parent class implementation + /// @note all launch algorithms are implemented in LaunchControllerAlgos.cpp virtual void calc_launch_algo(const VectornavData_s &vn_data) = 0; + /// @brief all launch controllers share the same evaluate method implemented in this class implementation. + /// @note refer to parent class for function documentation TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; #endif // __BASELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 5e5c962f2..e0bc09c5a 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -148,10 +148,18 @@ class CASESystem DrivetrainCommand_s get_current_drive_command() { return current_command_; } + /// @brief uses pedal data to determine the torque to be requested from the motors + /// pedals_data.accelPercent - pedals_data.regenPercent -> where accelpercent is to what percent the acellerator is pushed and the regen percent is the amount of regenerative braking currently applied + /// @param pedals_data has accel and regen percent + /// @param max_torque not used right now + /// @param max_regen_torque used for calculation of torque request + /// @param max_rpm not used right now + /// @return float representing the calculated request float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_torque, float max_regen_torque, float max_rpm); - /// @brief configuration function to determine what CASE is using / turn on and off different features within CASE - /// @param config the configuration struct we will be setting + /// @brief retrieves rpm setpoint based on final torque value + /// @param float final_torque + /// @return The maximum RPM from the case configuration if torque is positive, otherwise 0. float get_rpm_setpoint(float final_torque) { if (final_torque > 0) diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 1787cbb12..0071acf3e 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -268,10 +268,10 @@ DrivetrainCommand_s CASESystem::evaluate( DrivetrainCommand_s command; - command.torqueSetpoints[0] = res.FinalTorqueFL; - command.torqueSetpoints[1] = res.FinalTorqueFR; - command.torqueSetpoints[2] = res.FinalTorqueRL; - command.torqueSetpoints[3] = res.FinalTorqueRR; + command.inverter_torque_limit[0] = res.FinalTorqueFL; + command.inverter_torque_limit[1] = res.FinalTorqueFR; + command.inverter_torque_limit[2] = res.FinalTorqueRL; + command.inverter_torque_limit[3] = res.FinalTorqueRR; command.speeds_rpm[0] = get_rpm_setpoint(res.FinalTorqueFL); command.speeds_rpm[1] = get_rpm_setpoint(res.FinalTorqueFR); diff --git a/lib/systems/include/Controllers/CASEControllerWrapper.h b/lib/systems/include/Controllers/CASEControllerWrapper.h index 8e0dbd7e8..5931780ac 100644 --- a/lib/systems/include/Controllers/CASEControllerWrapper.h +++ b/lib/systems/include/Controllers/CASEControllerWrapper.h @@ -5,14 +5,18 @@ #include "CASESystem.h" template +/// @brief makes CASE system a part of Controller hierarchy for use in TC Mux class TorqueControllerCASEWrapper : public virtual Controller { public: TorqueControllerCASEWrapper() = delete; - + /// @param case_instance requires current state estimator instance to give access of the CASE instance to this specific controller so that it can update/control it. + /// @note This also ensures there are not duplicates of this system for safety. TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) { } + /// @brief packages CASE system command into Torque Controller Output + /// @param state this state is updated by the CASE system in main repeatedly TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { DrivetrainCommand_s curr_cmd = case_instance_->get_current_drive_command(); diff --git a/lib/systems/include/Controllers/LoadCellVectoringController.h b/lib/systems/include/Controllers/LoadCellVectoringController.h index dc7aaa373..9c7b59d47 100644 --- a/lib/systems/include/Controllers/LoadCellVectoringController.h +++ b/lib/systems/include/Controllers/LoadCellVectoringController.h @@ -57,12 +57,18 @@ class TorqueControllerLoadCellVectoring : public virtual Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = false; } + /// @brief default contructor with balanced default values: rearTorqueScale = 1.0, regenTorqueScale = 1.0 TorqueControllerLoadCellVectoring() : TorqueControllerLoadCellVectoring(1.0, 1.0) {} + /// @brief Calculates speed set point based on normal force applied to wheels individually + /// @param SysTick_s &tick + /// @param PedalsSystemData_s &pedalsData + /// @param LoadCellInterfaceOutput_s &loadCellData void tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const LoadCellInterfaceOutput_s &loadCellData); + /// @note refer to parent class for function documentation TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/LookupLaunchController.h b/lib/systems/include/Controllers/LookupLaunchController.h index 9072005fd..3953289f6 100644 --- a/lib/systems/include/Controllers/LookupLaunchController.h +++ b/lib/systems/include/Controllers/LookupLaunchController.h @@ -22,9 +22,9 @@ class TorqueControllerLookupLaunch : public virtual BaseLaunchController */ TorqueControllerLookupLaunch(int16_t initial_speed_target) : BaseLaunchController(initial_speed_target) {} - + /// @brief default constructor for slip launch controller: DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - + /// @brief increases speed target based on distance from start to ensure the speed target is progressing as the car begins to move void calc_launch_algo(const VectornavData_s &vn_data) override; }; diff --git a/lib/systems/include/Controllers/SimpleController.h b/lib/systems/include/Controllers/SimpleController.h index 33f691fee..16d67c1db 100644 --- a/lib/systems/include/Controllers/SimpleController.h +++ b/lib/systems/include/Controllers/SimpleController.h @@ -28,8 +28,10 @@ class TorqueControllerSimple : public Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = true; } - + /// @brief calculates torque output based off max torque and simple torque scaling metric defined in constructor and adjusts writeout_ accordingly + /// @param pedalsData controller calculates acceleration request from the pedals based off of this data void tick(const PedalsSystemData_s &pedalsData); + /// @note refer to parent class for function documentation TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/SimpleLaunchController.h b/lib/systems/include/Controllers/SimpleLaunchController.h index ef4f84a64..0d04df908 100644 --- a/lib/systems/include/Controllers/SimpleLaunchController.h +++ b/lib/systems/include/Controllers/SimpleLaunchController.h @@ -19,8 +19,8 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController public: /*! SIMPLE LAUNCH CONTROLLER - This launch controller is based off of a specified launch rate and an initial speed target - It will ramp up the speed target linearlly over time to accelerate + @brief this launch controller is based off of a specified launch rate and an initial speed target + It will ramp up the speed target linearly over time to accelerate @param launch_rate specified launch rate in m/s^2 @param initial_speed_target the initial speed commanded to the wheels */ @@ -28,9 +28,10 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController : BaseLaunchController(initial_speed_target), launch_rate_target_(launch_rate) {} - + /// @brief base constructor with default values: Default_Launch_Rate = 11.76, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - + /// @brief Increases speed target during launch linearly based off launch rate provided in the constructor + /// @param vn_data this data is not necessary for this controller but is supplied according to the interface void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SIMPLELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/Controllers/SlipLaunchController.h b/lib/systems/include/Controllers/SlipLaunchController.h index f3b61eeb6..fd0e7c406 100644 --- a/lib/systems/include/Controllers/SlipLaunchController.h +++ b/lib/systems/include/Controllers/SlipLaunchController.h @@ -24,9 +24,10 @@ class TorqueControllerSlipLaunch : public virtual BaseLaunchController TorqueControllerSlipLaunch(float slip_ratio, int16_t initial_speed_target) : BaseLaunchController(initial_speed_target), slip_ratio_(slip_ratio) {} - + /// @brief default constructor for slip launch controller: DEFAULT_SLIP_RATIO = 0.2, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - + /// @brief Increases speed target during launch linearly according to slip ratio to keep the cars wheels spinning faster than the velocity for increased traction + /// @param vn_data This controller needs velocity data for to keep updating increasing the speed according to the slip ratio void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index de01cc702..5c2b52356 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -148,7 +148,7 @@ void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_ int index = 0; for (auto inv_pointer : inverters_) { - inv_pointer->handle_command({data.torqueSetpoints[index], data.speeds_rpm[index]}); + inv_pointer->handle_command({data.inverter_torque_limit[index], data.speeds_rpm[index]}); index++; } // last_general_cmd_time_ = curr_system_millis_; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 1db0cfb80..30f646f4f 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -46,11 +46,13 @@ // - [ ] write testing code for this in separate environment +/// @brief Contains a max speed for mode changes(5 m/s), a max torque delta for mode change(.5 nm) and a max power limit(63000 W). +/// These values are used in the event that no value is provided for them in the constructor. namespace TC_MUX_DEFAULT_PARAMS { constexpr const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s constexpr const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm - constexpr const float MAX_POWER_LIMIT = 63000.0; + constexpr const float MAX_POWER_LIMIT = 63000.0; // watts of mechanical power }; template @@ -71,14 +73,33 @@ class TorqueControllerMux {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0f}}; float max_change_speed_, max_torque_pos_change_delta_, max_power_limit_; DrivetrainCommand_s prev_command_ = {}; - TorqueControllerMuxStatus current_status_ = {}; - TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + TorqueControllerMuxStatus active_status_ = {}; + TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out); + /// @brief Clamps negative rpms to 0f + /// @param const DrivetrainCommand_s &command provides the rpm info as a DrivetrainCommand_s + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); + + /// @brief Ensure torque is at most at the specified limit. If exceeding, then limit it in the returned DrivetrainCommand_s + /// @param const DrivetrainCommand_s &command is a DrivetrainCommand_s, which provides torque info + /// @param float max_torque this is the maximum average torque the wheels are allowed to experience before it is limited. + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); + + /// @brief Apply power limit (watts) such that the mechanical power of all wheels never exceeds the preset mechanical power limit. Scales all wheels down to preserve functionality of torque controllers + /// @param const DrivetrainCommand_s &command provides torque info, which is used to calculate mechanical power + /// @param const DrivetrainDynamicReport_s &drivetrain provides RPMS, which are used to calculate radians / s + /// @param float max_torque is used to indirectly specifiy the max power + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); + + /// @brief begin limiting regen at noRegenLimitKPH (hardcoded in func) and completely limit regen at fullRegenLimitKPH (hardcoded in func) + /// @param const DrivetrainCommand_s &command + /// @param const DrivetrainDynamicReport_s &drivetrain_data provides RPMs + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); public: @@ -90,6 +111,7 @@ class TorqueControllerMux /// @param max_change_speed the max speed difference between the requested controller output and the actual speed of each wheel that if the controller has a diff larger than the mux will not switch to the requested controller /// @param max_torque_pos_change_delta same as speed but evaluated between the controller commanded torques defaults to TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE /// @param max_power_limit the max power limit defaults to TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT + /// @note TC Mux must be created with at least 1 controller. explicit TorqueControllerMux(std::array controller_pointers, std::array mux_bypass_limits, float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, @@ -106,13 +128,13 @@ class TorqueControllerMux } - const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } + const TorqueControllerMuxStatus &get_tc_mux_status() { return active_status_; } - /// @brief function that evaluates the mux, controllers and gets the current command + /// @brief function that evaluates the mux, controllers and gets the active command /// @param requested_controller_type the requested controller type from the dial state /// @param controller_command_torque_limit the torque limit state enum set by dashboard - /// @param input_state the current state of the car - /// @return the current drivetrain command to be sent to the drivetrain + /// @param input_state the active state of the car + /// @return the active DrivetrainCommand_s to be sent to the drivetrain to command increases and decreases in torque DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e controller_command_torque_limit, const SharedCarState_s &input_state); @@ -123,4 +145,4 @@ const int number_of_controllers = 5; using TCMuxType = TorqueControllerMux; #include "TorqueControllerMux.tpp" -#endif // __TorqueControllerMux_H__ \ No newline at end of file +#endif // __TorqueControllerMux_H__ diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index c392240bd..4d2e9699a 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -13,24 +13,24 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C .ready = true}; int req_controller_mode_index = static_cast(requested_controller_type); - int current_controller_mode_index = static_cast(current_status_.current_controller_mode); + int active_controller_mode_index = static_cast(active_status_.active_controller_mode); if ((std::size_t)req_controller_mode_index > ( controller_pointers_.size() - 1 )) { - current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; + active_status_.active_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; return empty_command; } - if( (!(controller_pointers_[current_controller_mode_index])) || (!controller_pointers_[req_controller_mode_index])) + if( (!(controller_pointers_[active_controller_mode_index])) || (!controller_pointers_[req_controller_mode_index])) { - current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER; + active_status_.active_error = TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER; return empty_command; } - current_output = controller_pointers_[current_controller_mode_index]->evaluate(input_state); + current_output = controller_pointers_[active_controller_mode_index]->evaluate(input_state); - // std::cout << "output torques " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; - bool requesting_controller_change = requested_controller_type != current_status_.current_controller_mode; + // std::cout << "output torques " << current_output.command.inverter_torque_limit[0] << " " << current_output.command.inverter_torque_limit[1] << " " << current_output.command.inverter_torque_limit[2] << " " << current_output.command.inverter_torque_limit[3] << std::endl; + bool requesting_controller_change = requested_controller_type != active_status_.active_controller_mode; if (requesting_controller_change) { @@ -38,34 +38,34 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); if (error_state == TorqueControllerMuxError::NO_ERROR) { - current_status_.current_controller_mode = requested_controller_type; - current_controller_mode_index = req_controller_mode_index; + active_status_.active_controller_mode = requested_controller_type; + active_controller_mode_index = req_controller_mode_index; current_output = proposed_output; } - current_status_.current_error = error_state; + active_status_.active_error = error_state; } - if (!mux_bypass_limits_[current_controller_mode_index]) + if (!mux_bypass_limits_[active_controller_mode_index]) { - current_status_.current_torque_limit_enum = requested_torque_limit; + active_status_.active_torque_limit_enum = requested_torque_limit; current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); current_output.command = apply_torque_limit_(current_output.command, torque_limit_map_[requested_torque_limit]); - current_status_.current_torque_limit_value = torque_limit_map_[requested_torque_limit]; + active_status_.active_torque_limit_value = torque_limit_map_[requested_torque_limit]; current_output.command = apply_power_limit_(current_output.command, input_state.drivetrain_data, max_power_limit_, torque_limit_map_[requested_torque_limit]); current_output.command = apply_positive_speed_limit_(current_output.command); - current_status_.output_is_bypassing_limits = false; + active_status_.output_is_bypassing_limits = false; } else{ - current_status_.current_torque_limit_enum = TorqueLimit_e::TCMUX_FULL_TORQUE; - current_status_.current_torque_limit_value= PhysicalParameters::AMK_MAX_TORQUE; - current_status_.output_is_bypassing_limits = true; + active_status_.active_torque_limit_enum = TorqueLimit_e::TCMUX_FULL_TORQUE; + active_status_.active_torque_limit_value= PhysicalParameters::AMK_MAX_TORQUE; + active_status_.output_is_bypassing_limits = true; } - // std::cout << "output torques before return " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; + // std::cout << "output torques before return " << current_output.command.inverter_torque_limit[0] << " " << current_output.command.inverter_torque_limit[1] << " " << current_output.command.inverter_torque_limit[2] << " " << current_output.command.inverter_torque_limit[3] << std::endl; return current_output.command; } template -TorqueControllerMuxError TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, +TorqueControllerMuxError TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out) { @@ -74,9 +74,9 @@ TorqueControllerMuxError TorqueControllerMux::can_switch_contro bool torqueDeltaPreventsModeChange = false; for (int i = 0; i < NUM_MOTORS; i++) { - speedPreventsModeChange = (abs(current_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= max_change_speed_); + speedPreventsModeChange = (abs(active_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= max_change_speed_); // only if the torque delta is positive do we not want to switch to the new one - torqueDeltaPreventsModeChange = (desired_controller_out.torqueSetpoints[i] - previous_controller_command.torqueSetpoints[i]) > max_torque_pos_change_delta_; + torqueDeltaPreventsModeChange = (desired_controller_out.inverter_torque_limit[i] - previous_controller_command.inverter_torque_limit[i]) > max_torque_pos_change_delta_; if (speedPreventsModeChange) { return TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH; @@ -111,7 +111,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(co for (int i = 0; i < NUM_MOTORS; i++) { - avg_torque += abs(out.torqueSetpoints[i]); + avg_torque += abs(out.inverter_torque_limit[i]); } avg_torque /= NUM_MOTORS; @@ -123,7 +123,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(co // divide by scale to lower avg below max torque for (int i = 0; i < NUM_MOTORS; i++) { - out.torqueSetpoints[i] = out.torqueSetpoints[i] / scale; + out.inverter_torque_limit[i] = out.inverter_torque_limit[i] / scale; } } return out; @@ -146,9 +146,9 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con { // get the total magnitude of torque from all 4 wheels // sum up net torque - net_torque_mag += abs(out.torqueSetpoints[i]); + net_torque_mag += abs(out.inverter_torque_limit[i]); // calculate P = T*w for each wheel and sum together - net_power += abs(out.torqueSetpoints[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + net_power += abs(out.inverter_torque_limit[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); } // only evaluate power limit if current power exceeds it @@ -162,17 +162,17 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); // calculate the percent of total torque requested per wheel - float torque_percent = abs(out.torqueSetpoints[i] / net_torque_mag); + float torque_percent = abs(out.inverter_torque_limit[i] / net_torque_mag); // based on the torque percent and max power limit, get the max power each wheel can use float power_per_corner = (torque_percent * power_limit); // std::cout <<"corner power " << power_per_corner <::apply_regen_limit_(con { for (int i = 0; i < NUM_MOTORS; i++) { - out.torqueSetpoints[i] *= torqueScaleDown; + out.inverter_torque_limit[i] *= torqueScaleDown; } } return out; diff --git a/lib/systems/src/Controllers/BaseLaunchController.cpp b/lib/systems/src/Controllers/BaseLaunchController.cpp index c0bd646db..5a1ffab6c 100644 --- a/lib/systems/src/Controllers/BaseLaunchController.cpp +++ b/lib/systems/src/Controllers/BaseLaunchController.cpp @@ -31,10 +31,10 @@ void BaseLaunchController::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = brake_torque_req; - writeout_.command.torqueSetpoints[FR] = brake_torque_req; - writeout_.command.torqueSetpoints[RL] = brake_torque_req; - writeout_.command.torqueSetpoints[RR] = brake_torque_req; + writeout_.command.inverter_torque_limit[FL] = brake_torque_req; + writeout_.command.inverter_torque_limit[FR] = brake_torque_req; + writeout_.command.inverter_torque_limit[RL] = brake_torque_req; + writeout_.command.inverter_torque_limit[RR] = brake_torque_req; // init launch vars launch_speed_target_ = 0; @@ -53,10 +53,10 @@ void BaseLaunchController::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = brake_torque_req; - writeout_.command.torqueSetpoints[FR] = brake_torque_req; - writeout_.command.torqueSetpoints[RL] = brake_torque_req; - writeout_.command.torqueSetpoints[RR] = brake_torque_req; + writeout_.command.inverter_torque_limit[FL] = brake_torque_req; + writeout_.command.inverter_torque_limit[FR] = brake_torque_req; + writeout_.command.inverter_torque_limit[RL] = brake_torque_req; + writeout_.command.inverter_torque_limit[RR] = brake_torque_req; // init launch vars launch_speed_target_ = 0; @@ -94,10 +94,10 @@ void BaseLaunchController::tick( writeout_.command.speeds_rpm[RL] = launch_speed_target_; writeout_.command.speeds_rpm[RR] = launch_speed_target_; - writeout_.command.torqueSetpoints[FL] = PhysicalParameters::AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[FR] = PhysicalParameters::AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RL] = PhysicalParameters::AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RR] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[FL] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[FR] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[RL] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[RR] = PhysicalParameters::AMK_MAX_TORQUE; break; } diff --git a/lib/systems/src/Controllers/LoadCellVectoringController.cpp b/lib/systems/src/Controllers/LoadCellVectoringController.cpp index e85502e84..5643df325 100644 --- a/lib/systems/src/Controllers/LoadCellVectoringController.cpp +++ b/lib/systems/src/Controllers/LoadCellVectoringController.cpp @@ -64,10 +64,10 @@ void TorqueControllerLoadCellVectoring::tick( writeout_.command.speeds_rpm[RL] = PhysicalParameters::AMK_MAX_RPM; writeout_.command.speeds_rpm[RR] = PhysicalParameters::AMK_MAX_RPM; - writeout_.command.torqueSetpoints[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; - writeout_.command.torqueSetpoints[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; - writeout_.command.torqueSetpoints[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; - writeout_.command.torqueSetpoints[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; + writeout_.command.inverter_torque_limit[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; + writeout_.command.inverter_torque_limit[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; + writeout_.command.inverter_torque_limit[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; + writeout_.command.inverter_torque_limit[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; } else { @@ -80,10 +80,10 @@ void TorqueControllerLoadCellVectoring::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RR] = torqueRequest * rearRegenTorqueScale_; } } else diff --git a/lib/systems/src/Controllers/SimpleController.cpp b/lib/systems/src/Controllers/SimpleController.cpp index 4906e4ed8..0457cef95 100644 --- a/lib/systems/src/Controllers/SimpleController.cpp +++ b/lib/systems/src/Controllers/SimpleController.cpp @@ -18,10 +18,10 @@ void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) writeout_.command.speeds_rpm[RL] = PhysicalParameters::AMK_MAX_RPM; writeout_.command.speeds_rpm[RR] = PhysicalParameters::AMK_MAX_RPM; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; + writeout_.command.inverter_torque_limit[FL] = torqueRequest * frontTorqueScale_; + writeout_.command.inverter_torque_limit[FR] = torqueRequest * frontTorqueScale_; + writeout_.command.inverter_torque_limit[RL] = torqueRequest * rearTorqueScale_; + writeout_.command.inverter_torque_limit[RR] = torqueRequest * rearTorqueScale_; } else { @@ -33,10 +33,10 @@ void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RR] = torqueRequest * rearRegenTorqueScale_; } } diff --git a/src/main.cpp b/src/main.cpp index a8c2fe435..0f7c2e608 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -426,7 +426,7 @@ void loop() tick_all_systems(curr_tick); - // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode), curr_tick.millis, 100); + // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode), curr_tick.millis, 100); // inverter procedure before entering state machine // reset inverters if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) @@ -481,9 +481,9 @@ void loop() Serial.print("Filtered max cell temp: "); Serial.println(ams_interface.get_filtered_max_cell_temp()); Serial.print("Current TC index: "); - Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode)); + Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode)); Serial.print("Current TC error: "); - Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); + Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_error)); Serial.println(); Serial.println(); } @@ -530,10 +530,10 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) int(fsm.get_state()), buzzer.buzzer_is_on(), drivetrain.drivetrain_error_occured(), - torque_controller_mux.get_tc_mux_status().current_torque_limit_enum, + torque_controller_mux.get_tc_mux_status().active_torque_limit_enum, ams_interface.get_filtered_min_cell_voltage(), a1.get().conversions[MCU15_GLV_SENSE_CHANNEL], - static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode), + static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode), dashboard.getDialMode()); main_ecu.tick( @@ -566,7 +566,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE1_CHANNEL], a1.get().conversions[MCU15_BRAKE2_CHANNEL], pedals_system.getMechBrakeActiveThreshold(), - torque_controller_mux.get_tc_mux_status().current_error); + torque_controller_mux.get_tc_mux_status().active_error); } if (t.trigger50) // 50Hz diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 7d7d1beae..5347a6745 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -4,6 +4,7 @@ #include "TorqueControllerMux.h" #include "TorqueControllers.h" #include "fake_controller_type.h" +#include "gtest/gtest.h" @@ -25,10 +26,10 @@ void set_outputs(controller_type &controller, float mps, float torque) controller.output.command.speeds_rpm[1] = METERS_PER_SECOND_TO_RPM * mps; controller.output.command.speeds_rpm[2] = METERS_PER_SECOND_TO_RPM * mps; controller.output.command.speeds_rpm[3] = METERS_PER_SECOND_TO_RPM * mps; - controller.output.command.torqueSetpoints[0] = torque; - controller.output.command.torqueSetpoints[1] = torque; - controller.output.command.torqueSetpoints[2] = torque; - controller.output.command.torqueSetpoints[3] = torque; + controller.output.command.inverter_torque_limit[0] = torque; + controller.output.command.inverter_torque_limit[1] = torque; + controller.output.command.inverter_torque_limit[2] = torque; + controller.output.command.inverter_torque_limit[3] = torque; } template void set_output_rpm(controller_type &controller, float rpm, float torque) @@ -37,10 +38,10 @@ void set_output_rpm(controller_type &controller, float rpm, float torque) controller.output.command.speeds_rpm[1] = rpm; controller.output.command.speeds_rpm[2] = rpm; controller.output.command.speeds_rpm[3] = rpm; - controller.output.command.torqueSetpoints[0] = torque; - controller.output.command.torqueSetpoints[1] = torque; - controller.output.command.torqueSetpoints[2] = torque; - controller.output.command.torqueSetpoints[3] = torque; + controller.output.command.inverter_torque_limit[0] = torque; + controller.output.command.inverter_torque_limit[1] = torque; + controller.output.command.inverter_torque_limit[2] = torque; + controller.output.command.inverter_torque_limit[3] = torque; } TEST(TorqueControllerMuxTesting, test_construction) { @@ -55,12 +56,12 @@ TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) SharedCarState_s state({}, {}, {}, {}, {}, {}); auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); for (int i =0; i< 4; i++) { ASSERT_EQ(res.speeds_rpm[i], 0.0); - ASSERT_EQ(res.torqueSetpoints[i], 0.0); + ASSERT_EQ(res.inverter_torque_limit[i], 0.0); } } @@ -84,8 +85,8 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) // auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH); + ASSERT_EQ(test.get_tc_mux_status().active_controller_mode, ControllerMode_e::MODE_0); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH); set_outputs(inst1, 0, 1); set_outputs(inst2, 0, 1); @@ -93,8 +94,8 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_1); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(test.get_tc_mux_status().active_controller_mode, ControllerMode_e::MODE_1); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::NO_ERROR); } TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) @@ -107,8 +108,8 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); + ASSERT_EQ(test.get_tc_mux_status().active_controller_mode, ControllerMode_e::MODE_0); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); // tick it a bunch of times out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); @@ -118,14 +119,14 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); + ASSERT_EQ(test.get_tc_mux_status().active_controller_mode, ControllerMode_e::MODE_0); - ASSERT_EQ(out1.torqueSetpoints[0], 1); - ASSERT_EQ(out1.torqueSetpoints[1], 1); - ASSERT_EQ(out1.torqueSetpoints[2], 1); - ASSERT_EQ(out1.torqueSetpoints[3], 1); + ASSERT_EQ(out1.inverter_torque_limit[0], 1); + ASSERT_EQ(out1.inverter_torque_limit[1], 1); + ASSERT_EQ(out1.inverter_torque_limit[2], 1); + ASSERT_EQ(out1.inverter_torque_limit[3], 1); } TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) @@ -179,17 +180,17 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) SharedCarState_s mode_0_input_state({}, {}, {}, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); DrivetrainCommand_s out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); - ASSERT_NEAR(out.torqueSetpoints[0], (max_torque / 2), 0.01); - ASSERT_NEAR(out.torqueSetpoints[1], (max_torque / 2), 0.01); - ASSERT_NEAR(out.torqueSetpoints[2], (max_torque / 2), 0.01); - ASSERT_NEAR(out.torqueSetpoints[3], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[0], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[1], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[2], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[3], (max_torque / 2), 0.01); mode_0_input_state = {{}, {}, {}, {}, {.accelPercent = 0.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}}; out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); - ASSERT_EQ(out.torqueSetpoints[0], 0); - ASSERT_EQ(out.torqueSetpoints[1], 0); - ASSERT_EQ(out.torqueSetpoints[2], 0); - ASSERT_EQ(out.torqueSetpoints[3], 0); + ASSERT_EQ(out.inverter_torque_limit[0], 0); + ASSERT_EQ(out.inverter_torque_limit[1], 0); + ASSERT_EQ(out.inverter_torque_limit[2], 0); + ASSERT_EQ(out.inverter_torque_limit[3], 0); // out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_1_input_state); } @@ -211,7 +212,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) for (int i = 0; i < 4; i++) { - ASSERT_EQ(res.torqueSetpoints[i], 10.0f); + ASSERT_EQ(res.inverter_torque_limit[i], 10.0f); } for (int i = 0; i < 4; i++) { @@ -224,7 +225,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) for (int i = 0; i < 4; i++) { - ASSERT_LT(res.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator + ASSERT_LT(res.inverter_torque_limit[i], 7.6); // hardcoded value based on online calculator } } @@ -234,7 +235,7 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) TestControllerType inst1; set_output_rpm(inst1, 500, 10.0); - inst1.output.command.torqueSetpoints[0] = 5; + inst1.output.command.inverter_torque_limit[0] = 5; TorqueControllerMux<1> test({static_cast(&inst1)}, {false}); DrivetrainDynamicReport_s drivetrain_data = {}; @@ -247,25 +248,25 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) auto drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); - ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); - ASSERT_EQ(drive_command.torqueSetpoints[1], 10.0f); - ASSERT_EQ(drive_command.torqueSetpoints[2], 10.0f); - ASSERT_EQ(drive_command.torqueSetpoints[3], 10.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[0], 5.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[1], 10.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[2], 10.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[3], 10.0f); set_output_rpm(inst1, 500, 20.0); - inst1.output.command.torqueSetpoints[0] = 5; + inst1.output.command.inverter_torque_limit[0] = 5; drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); - ASSERT_LT(drive_command.torqueSetpoints[0], 3.5f); - ASSERT_LT(drive_command.torqueSetpoints[1], 12.5f); - ASSERT_LT(drive_command.torqueSetpoints[2], 12.5f); - ASSERT_LT(drive_command.torqueSetpoints[3], 12.5f); + ASSERT_LT(drive_command.inverter_torque_limit[0], 3.5f); + ASSERT_LT(drive_command.inverter_torque_limit[1], 12.5f); + ASSERT_LT(drive_command.inverter_torque_limit[2], 12.5f); + ASSERT_LT(drive_command.inverter_torque_limit[3], 12.5f); - printf("torque 1: %.2f\n", drive_command.torqueSetpoints[0]); - printf("torque 2: %.2f\n", drive_command.torqueSetpoints[1]); - printf("torque 3: %.2f\n", drive_command.torqueSetpoints[2]); - printf("torque 4: %.2f\n", drive_command.torqueSetpoints[3]); + printf("torque 1: %.2f\n", drive_command.inverter_torque_limit[0]); + printf("torque 2: %.2f\n", drive_command.inverter_torque_limit[1]); + printf("torque 3: %.2f\n", drive_command.inverter_torque_limit[2]); + printf("torque 4: %.2f\n", drive_command.inverter_torque_limit[3]); } TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) @@ -275,9 +276,9 @@ TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, state); for (int i = 0; i < 4; i++) { - ASSERT_EQ(res.torqueSetpoints[i], 0.0f); + ASSERT_EQ(res.inverter_torque_limit[i], 0.0f); ASSERT_EQ(res.speeds_rpm[i], 0.0f); } - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); } #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file