From 91bf533d78b226b0c2289360e16c4a7e03aa8115 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 6 Sep 2024 00:16:36 -0400 Subject: [PATCH] adding in latching logic for the db controller --- lib/systems/include/DrivebrainController.h | 17 +- lib/systems/include/TorqueControllerMux.h | 15 +- lib/systems/include/TorqueControllers.h | 104 ++-- lib/systems/src/DrivebrainController.cpp | 15 +- lib/systems/src/TorqueControllerMux.cpp | 13 +- lib/systems/src/TorqueControllers.cpp | 92 ++-- .../test_systems/torque_controller_mux_test.h | 504 +++++++++--------- 7 files changed, 403 insertions(+), 357 deletions(-) diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 14ad6dc09..83d316214 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -6,12 +6,22 @@ #include "DrivebrainData.h" // TODO - [ ] need to validate that the times that are apparent in the drivebrain data -// and ensure that they are within tolerence to current sys-tick -// TODO - [ ] set +// and ensure that they are within tolerence to current sys-tick + +// TODO - [ ] if the drivebrain controller is currently the active controller, +// the latency fault should be latching + +// meaning that if we fail any of the latency checks while active we cannot clear the timing failure fault +// unless we switch to another controller and back again. in reality, the CAN line or ethernet conditions +// probably wont improve randomly during runtime so it will keep faulting. primarily this is just to make sure +// we dont keep going from latent to not latent while driving and thus the driver gets jittered and the +// drivetrain will keep "powering up" and "powering down" + + class DrivebrainController : public TorqueController { public: - void tick(const SysTick_s &sys_tick, DrivebrainData db_input); + void tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller); DrivebrainController(TorqueControllerOutput_s &writeout, unsigned long allowed_latency, unsigned long allowed_jitter) : _writeout(writeout) { @@ -25,6 +35,7 @@ class DrivebrainController : public TorqueController unsigned long allowed_latency; } _params; + bool _timing_failure = false; unsigned long _last_sent_torque_lim_millis = -1; unsigned long _last_sent_speed_setpoint_millis = -1; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index f56731ba1..ac04971f5 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -12,6 +12,7 @@ #include "VectornavInterface.h" #include "LoadCellInterface.h" #include "TelemetryInterface.h" +#include "DrivebrainController.h" const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm @@ -24,16 +25,17 @@ class TorqueControllerMux TorqueControllerSimple torqueControllerSimple_; TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_; - TorqueControllerSlipLaunch torqueControllerSlipLaunch_; + DrivebrainController _dbController; TorqueControllerCASEWrapper tcCASEWrapper_; + // Use this to map the dial to TCMUX modes std::unordered_map dialModeMap_ = { {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, {DialMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING}, {DialMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM}, {DialMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH}, - {DialMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH}, + {DialMode_e::MODE_4, TorqueController_e::TC_DRIVEBRAIN}, {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, }; std::unordered_map torqueLimitMap_ = { @@ -53,7 +55,7 @@ class TorqueControllerMux static_cast(&torqueControllerSimple_), static_cast(&torqueControllerLoadCellVectoring_), static_cast(&torqueControllerSimpleLaunch_), - static_cast(&torqueControllerSlipLaunch_), + static_cast(&_dbController), static_cast(&tcCASEWrapper_) }; @@ -73,7 +75,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)]) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) - , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) + , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 30, 10) , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) , telemHandle_(telemInterface) {} @@ -86,7 +88,7 @@ class TorqueControllerMux , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) - , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) + , _dbController(controllerOutputs_[static_cast(TorqueController_e::TC_DRIVEBRAIN)], 30, 10) , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) , telemHandle_(telemInterface) {} @@ -102,7 +104,8 @@ class TorqueControllerMux float accDerateFactor, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, - const DrivetrainCommand_s &CASECommand + const DrivetrainCommand_s &CASECommand, + DrivebrainData db_input ); /// @brief apply corresponding limits on drivetrain command calculated by torque controller diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 54d0ea39b..284412802 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -73,11 +73,9 @@ enum TorqueController_e TC_SAFE_MODE = 1, TC_LOAD_CELL_VECTORING = 2, TC_SIMPLE_LAUNCH = 3, - TC_SLIP_LAUNCH = 4, - TC_LOOKUP_LAUNCH = 5, - TC_CASE_SYSTEM = 6, - TC_DRIVEBRAIN= 7, - TC_NUM_CONTROLLERS = 8, + TC_CASE_SYSTEM = 4, + TC_DRIVEBRAIN= 5, + TC_NUM_CONTROLLERS = 6, }; enum class LaunchStates_e @@ -267,54 +265,54 @@ class TorqueControllerSimpleLaunch : public TorqueController, void calc_launch_algo(const vector_nav *vn_data) override; }; -class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController -{ -private: - float slip_ratio_; - -public: - /*! - SLIP LAUNCH CONTROLLER - This launch controller is based off of a specified slip constant. It will at all times attempt - to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it - in constant slip - @param slip_ratio specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target), - slip_ratio_(slip_ratio) {} - - TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; - -class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController -{ -private: - bool init_position = false; - -public: - /*! - Lookup Launch Controller - This launch controller is based off of a matlab and symlink generated lookup table. - This has been converted to a C array with some basic python code using the array index - as the input for the controller - @param slip_ratio specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target) {} - - TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; +// class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController +// { +// private: +// float slip_ratio_; + +// public: +// /*! +// SLIP LAUNCH CONTROLLER +// This launch controller is based off of a specified slip constant. It will at all times attempt +// to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it +// in constant slip +// @param slip_ratio specified launch rate in m/s^2 +// @param initial_speed_target the initial speed commanded to the wheels +// */ +// TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target) +// : BaseLaunchController(writeout, initial_speed_target), +// slip_ratio_(slip_ratio) {} + +// TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {} + +// LaunchStates_e get_launch_state() override { return launch_state_; } + +// void calc_launch_algo(const vector_nav *vn_data) override; +// }; + +// class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController +// { +// private: +// bool init_position = false; + +// public: +// /*! +// Lookup Launch Controller +// This launch controller is based off of a matlab and symlink generated lookup table. +// This has been converted to a C array with some basic python code using the array index +// as the input for the controller +// @param slip_ratio specified launch rate in m/s^2 +// @param initial_speed_target the initial speed commanded to the wheels +// */ +// TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) +// : BaseLaunchController(writeout, initial_speed_target) {} + +// TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {} + +// LaunchStates_e get_launch_state() override { return launch_state_; } + +// void calc_launch_algo(const vector_nav *vn_data) override; +// }; class TorqueControllerCASEWrapper : public TorqueController { diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index cb9268aa6..1cbd819ce 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,17 +1,25 @@ #include "DrivebrainController.h" -void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input) +void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_input, bool is_active_controller) { _writeout.ready = false; bool speed_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_speed_setpoint_receive_time_millis)) > (int)_params.allowed_latency); bool torque_setpoint_too_latent = (::abs((int)(sys_tick.millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_latency); bool msg_jitter_too_high = (::abs((int)(db_input.last_speed_setpoint_receive_time_millis - db_input.last_torque_lim_receive_time_millis)) > (int)_params.allowed_jitter); + + bool timing_failure = (speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high); + + // only in the case that this is the active controller do we want to clear our timing failure + if((!is_active_controller) && (!timing_failure) ) + { + // timing failure should be false here + _timing_failure = timing_failure; + } - if (!(speed_setpoint_too_latent || torque_setpoint_too_latent || msg_jitter_too_high)) + if (!timing_failure) { _writeout.ready = true; - _last_sent_speed_setpoint_millis = db_input.last_speed_setpoint_receive_time_millis; _last_sent_torque_lim_millis = db_input.last_torque_lim_receive_time_millis; @@ -20,6 +28,7 @@ void DrivebrainController::tick(const SysTick_s &sys_tick, DrivebrainData db_inp } else { + _timing_failure = true; _writeout.ready = false; _writeout.command = {}; // set command to all zeros if bad latency is apparent } diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index e6e04fa8b..5c5dfe347 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -1,6 +1,7 @@ #include "TorqueControllerMux.h" #include "Utility.h" #include "PhysicalParameters.h" +#include "DrivebrainController.h" void TorqueControllerMux::tick( const SysTick_s &tick, @@ -12,13 +13,16 @@ void TorqueControllerMux::tick( float accDerateFactor, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, - const DrivetrainCommand_s &CASECommand) + const DrivetrainCommand_s &CASECommand, + DrivebrainData db_input) { // Tick all torque controllers torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]); torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadCellData); torqueControllerSimpleLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); - torqueControllerSlipLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); + + bool drivebrain_in_control = (muxMode_ == TorqueController_e::TC_DRIVEBRAIN); + _dbController.tick(tick, db_input, drivebrain_in_control); tcCASEWrapper_.tick( (TCCaseWrapperTick_s){ .command = CASECommand, @@ -106,13 +110,12 @@ void TorqueControllerMux::tick( // Apply setpoints value limits // Derating for endurance - - if (muxMode_ != TC_CASE_SYSTEM) + if (muxMode_ != TorqueController_e::TC_CASE_SYSTEM) { // Safety checks for CASE: CASE handles regen, torque, and power limit internally applyRegenLimit(&drivetrainCommand_, &drivetrainData); // Apply torque limit before power limit to not power limit - if ((muxMode_ != TC_SIMPLE_LAUNCH) && (muxMode_ != TC_SLIP_LAUNCH) && (muxMode_ != TC_LOOKUP_LAUNCH)) + if ((muxMode_ != TC_SIMPLE_LAUNCH)) { applyTorqueLimit(&drivetrainCommand_); } diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 9497c9b61..96942c28a 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -291,53 +291,53 @@ void TorqueControllerSimpleLaunch::calc_launch_algo(const vector_nav* vn_data) { launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); } -void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { - // accelerate at constant speed for a period of time to get body velocity up - // may want to make this the ht07 launch algo +// void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { +// // accelerate at constant speed for a period of time to get body velocity up +// // may want to make this the ht07 launch algo - // makes sure that the car launches at the target launch speed - launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET); - - /* - New slip-ratio based launch algorithm by Luke Chen. The basic idea - is to always be pushing the car a certain 'slip_ratio_' faster than - the car is currently going, theoretically always keeping the car in slip - */ - // m/s - float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); - // rpm - new_speed_target *= METERS_PER_SECOND_TO_RPM; - // makes sure the car target speed never goes lower than prev. target - // allows for the vn to 'spool' up and us to get reliable vx data - launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); -} - -void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { - - launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); - - double dx = vn_data->ecef_coords[0] - initial_ecef_x_; - double dy = vn_data->ecef_coords[1] - initial_ecef_y_; - double dz = vn_data->ecef_coords[2] - initial_ecef_z_; - - double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz)); - - /* - Distance-lookup launch algorithm. Takes in the vel_dist_lookup - generated from Luke's matlab/symlink to set speed targets based - on distance travelled from the start point. - This can also and may be better to replace with an integration - of body velocity. - */ - - uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10 - idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float))); - float mps_target = vel_dist_lookup[idx]; - - float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; - launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); - -} +// // makes sure that the car launches at the target launch speed +// launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET); + +// /* +// New slip-ratio based launch algorithm by Luke Chen. The basic idea +// is to always be pushing the car a certain 'slip_ratio_' faster than +// the car is currently going, theoretically always keeping the car in slip +// */ +// // m/s +// float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); +// // rpm +// new_speed_target *= METERS_PER_SECOND_TO_RPM; +// // makes sure the car target speed never goes lower than prev. target +// // allows for the vn to 'spool' up and us to get reliable vx data +// launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); +// } + +// void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { + +// launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); + +// double dx = vn_data->ecef_coords[0] - initial_ecef_x_; +// double dy = vn_data->ecef_coords[1] - initial_ecef_y_; +// double dz = vn_data->ecef_coords[2] - initial_ecef_z_; + +// double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz)); + +// /* +// Distance-lookup launch algorithm. Takes in the vel_dist_lookup +// generated from Luke's matlab/symlink to set speed targets based +// on distance travelled from the start point. +// This can also and may be better to replace with an integration +// of body velocity. +// */ + +// uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10 +// idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float))); +// float mps_target = vel_dist_lookup[idx]; + +// float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; +// launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); + +// } void TorqueControllerCASEWrapper::tick(const TCCaseWrapperTick_s &intake) { diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index 64a63b5f0..fa6d4d7dd 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -113,7 +113,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -135,7 +136,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // Now press the pedal again. Torque should be requested @@ -149,7 +151,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -229,7 +232,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -251,7 +255,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // Now press the pedal. Torque should be requested @@ -265,7 +270,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); @@ -455,7 +461,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // change mode to mode 3 @@ -469,7 +476,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); // tick again to calculate state switch @@ -483,7 +491,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -501,7 +510,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -519,7 +529,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -537,7 +548,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -554,7 +566,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); @@ -572,7 +585,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { 1.0, false, vn_data, - (const DrivetrainCommand_s) {} + (const DrivetrainCommand_s) {}, + {} ); launch_state = torque_controller_mux.activeController()->get_launch_state(); @@ -580,232 +594,240 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_NOT_READY); } -TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TelemetryInterface telem_interface; - TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s resulting_torque_command; - - vector_nav vn_data{}; - - DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_barely_launch_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0, 2785.86, 0, 0}, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - // DrivetrainDynamicReport_s simulated_no_launch_drivetrain_dynamics = { - // .measuredInverterFLPackVoltage = 550, - // .measuredSpeeds = {0, 2786.86, 0, 0}, - // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - // }; - - // DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { - // .measuredInverterFLPackVoltage = 550, - // .measuredSpeeds = { - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) - // }, - // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - // }; - - PedalsSystemData_s simulated_full_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_no_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 0.0, - .brakePercent = 0.0 - }; - - // PedalsSystemData_s simulated_accel_and_brake_press = { - // .accelImplausible = false, - // .brakeImplausible = false, - // .brakePressed = false, - // .brakeAndAccelPressedImplausibility = false, - // .implausibilityExceededMaxDuration = false, - // .accelPercent = 1.0, - // .brakePercent = 0.3 - // }; - - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // change mode to mode 3 - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick again to calculate state switch - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); - - // press accel now with one wheelspeed barely under threshold - torque_controller_mux.tick( - clock.tick(100000), - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick TCMUX (10k us will hit 100hz) - torque_controller_mux.tick( - clock.tick(100000), - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); - - torque_controller_mux.tick( - clock.tick(const_accel_time * 1000), // const accel time sine launch - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - commanded = torque_controller_mux.getDrivetrainCommand(); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); - - // if velocity is less than the default speed, it should still go at launch speed - vn_data.velocity_x = 0; // m/s - - torque_controller_mux.tick( - clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - commanded = torque_controller_mux.getDrivetrainCommand(); - printf("lower vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); - ASSERT_EQ(commanded.speeds_rpm[0] , DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target - - // this is approx. the speed the car would be going at 1500 rpm - // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this - vn_data.velocity_x = DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s - - torque_controller_mux.tick( - clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - commanded = torque_controller_mux.getDrivetrainCommand(); - printf("launch vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0] > (DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); - - - // this fails, but the value is close enough - // ASSERT_EQ((float)((int)((commanded.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f); -} +// TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { +// SysClock clock = SysClock(); +// SysTick_s cur_tick; +// cur_tick = clock.tick(0); +// TelemetryInterface telem_interface; +// TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); +// DrivetrainCommand_s resulting_torque_command; + +// vector_nav vn_data{}; + +// DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { +// .measuredInverterFLPackVoltage = 550, +// .measuredSpeeds = { +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), +// ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) +// }, +// .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// }; + +// DrivetrainDynamicReport_s simulated_barely_launch_drivetrain_dynamics = { +// .measuredInverterFLPackVoltage = 550, +// .measuredSpeeds = {0, 2785.86, 0, 0}, +// .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// }; + +// // DrivetrainDynamicReport_s simulated_no_launch_drivetrain_dynamics = { +// // .measuredInverterFLPackVoltage = 550, +// // .measuredSpeeds = {0, 2786.86, 0, 0}, +// // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// // }; + +// // DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { +// // .measuredInverterFLPackVoltage = 550, +// // .measuredSpeeds = { +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), +// // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) +// // }, +// // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, +// // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, +// // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} +// // }; + +// PedalsSystemData_s simulated_full_accel_press = { +// .accelImplausible = false, +// .brakeImplausible = false, +// .brakePressed = false, +// .brakeAndAccelPressedImplausibility = false, +// .implausibilityExceededMaxDuration = false, +// .accelPercent = 1.0, +// .brakePercent = 0.0 +// }; + +// PedalsSystemData_s simulated_no_accel_press = { +// .accelImplausible = false, +// .brakeImplausible = false, +// .brakePressed = false, +// .brakeAndAccelPressedImplausibility = false, +// .implausibilityExceededMaxDuration = false, +// .accelPercent = 0.0, +// .brakePercent = 0.0 +// }; + +// // PedalsSystemData_s simulated_accel_and_brake_press = { +// // .accelImplausible = false, +// // .brakeImplausible = false, +// // .brakePressed = false, +// // .brakeAndAccelPressedImplausibility = false, +// // .implausibilityExceededMaxDuration = false, +// // .accelPercent = 1.0, +// // .brakePercent = 0.3 +// // }; + +// torque_controller_mux.tick( +// cur_tick, +// simulated_slow_drivetrain_dynamics, +// simulated_no_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_0, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// // change mode to mode 3 +// torque_controller_mux.tick( +// cur_tick, +// simulated_slow_drivetrain_dynamics, +// simulated_no_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// // tick again to calculate state switch +// torque_controller_mux.tick( +// cur_tick, +// simulated_slow_drivetrain_dynamics, +// simulated_no_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); + +// ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); + +// // press accel now with one wheelspeed barely under threshold +// torque_controller_mux.tick( +// clock.tick(100000), +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// // tick TCMUX (10k us will hit 100hz) +// torque_controller_mux.tick( +// clock.tick(100000), +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// launch_state = torque_controller_mux.activeController()->get_launch_state(); + +// ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); + +// DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); +// ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); + +// torque_controller_mux.tick( +// clock.tick(const_accel_time * 1000), // const accel time sine launch +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// launch_state = torque_controller_mux.activeController()->get_launch_state(); + +// ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); + +// commanded = torque_controller_mux.getDrivetrainCommand(); +// ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); + +// // if velocity is less than the default speed, it should still go at launch speed +// vn_data.velocity_x = 0; // m/s + +// torque_controller_mux.tick( +// clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// commanded = torque_controller_mux.getDrivetrainCommand(); +// printf("lower vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); +// ASSERT_EQ(commanded.speeds_rpm[0] , DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target + +// // this is approx. the speed the car would be going at 1500 rpm +// // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this +// vn_data.velocity_x = DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s + +// torque_controller_mux.tick( +// clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time +// simulated_barely_launch_drivetrain_dynamics, +// simulated_full_accel_press, +// (const SteeringSystemData_s) {}, +// (const LoadCellInterfaceOutput_s) {}, +// DialMode_e::MODE_4, +// 1.0, +// false, +// vn_data, +// (const DrivetrainCommand_s) {}, +// {} +// ); + +// commanded = torque_controller_mux.getDrivetrainCommand(); +// printf("launch vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); +// ASSERT_EQ((int16_t)commanded.speeds_rpm[0] > (DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); + + +// // this fails, but the value is close enough +// // ASSERT_EQ((float)((int)((commanded.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f); +// } #endif /* TORQUE_CONTROLLER_MUX_TEST */ \ No newline at end of file