From f8a15dfb0c6354e0271409d195de45ab5e05ba39 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 9 May 2024 18:37:32 -0400 Subject: [PATCH 01/31] adding in initial revision of the tc mux rework --- README.md | 5 - include/MCU_rev15_defs.h | 4 +- lib/interfaces/include/DashboardInterface.h | 38 +--- lib/interfaces/include/ProtobufMsgInterface.h | 2 +- lib/interfaces/include/TelemetryInterface.h | 3 +- lib/interfaces/include/VectornavInterface.h | 40 ++--- lib/interfaces/include/VectornavInterface.tpp | 4 +- lib/interfaces/src/DashboardInterface.cpp | 6 +- lib/mock_interfaces/DashboardInterface.h | 24 +-- lib/mock_interfaces/VectornavInterface.h | 23 +-- .../include/PhysicalParameters.h | 0 lib/shared_data/include/SharedDataTypes.h | 115 ++++++++++++ .../include/TorqueControllersData.h | 14 -- .../include/Utility.h | 2 +- lib/systems/include/CASESystem.h | 2 +- lib/systems/include/CASESystem.tpp | 2 +- lib/systems/include/DrivetrainSystem.h | 15 +- lib/systems/include/PedalsSystem.h | 16 +- lib/systems/include/TorqueControllerMux.h | 22 +-- lib/systems/include/TorqueControllerMuxV2.h | 79 ++++++++ lib/systems/include/TorqueControllerMuxV2.tpp | 168 ++++++++++++++++++ lib/systems/include/TorqueControllers.h | 23 +-- lib/systems/src/TorqueControllerMux.cpp | 4 +- lib/systems/src/TorqueControllers.cpp | 8 +- platformio.ini | 33 ++-- test/test_systems/main.cpp | 4 +- test/test_systems/tc_mux_v2_testing.h | 54 ++++++ .../test_systems/torque_controller_mux_test.h | 54 +++--- 28 files changed, 535 insertions(+), 229 deletions(-) rename lib/{systems => shared_data}/include/PhysicalParameters.h (100%) create mode 100644 lib/shared_data/include/SharedDataTypes.h delete mode 100644 lib/shared_data/include/TorqueControllersData.h rename lib/{systems => shared_data}/include/Utility.h (95%) create mode 100644 lib/systems/include/TorqueControllerMuxV2.h create mode 100644 lib/systems/include/TorqueControllerMuxV2.tpp create mode 100644 test/test_systems/tc_mux_v2_testing.h diff --git a/README.md b/README.md index a0c890c3d..025f59097 100644 --- a/README.md +++ b/README.md @@ -349,10 +349,5 @@ flowchart LR nanopb --> MCU other_protos[other msgs.proto] other_protos --> h_proto - - - - - ``` \ No newline at end of file diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 590a9d52e..63a06c62a 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -2,9 +2,7 @@ #define __MCU15_H__ #include "PedalsSystem.h" -#ifndef TESTING_SYSTEMS -#include "InterfaceParams.h" -#endif + // pindefs diff --git a/lib/interfaces/include/DashboardInterface.h b/lib/interfaces/include/DashboardInterface.h index 494e83384..f79a318aa 100644 --- a/lib/interfaces/include/DashboardInterface.h +++ b/lib/interfaces/include/DashboardInterface.h @@ -6,36 +6,10 @@ #include "hytech.h" #include "MCUInterface.h" #include "InverterInterface.h" - +#include "SharedDataTypes.h" #include "TorqueControllers.h" -/* - Enum for the car's torque limits - MOVE ME! - ideally into a TorqueControllerDefs.h file - to prevent circular dependencies -*/ -enum class TorqueLimit_e -{ - TCMUX_LOW_TORQUE = 0, - TCMUX_MID_TORQUE = 1, - TCMUX_FULL_TORQUE = 2, - TCMUX_NUM_TORQUE_LIMITS = 3, -}; -/* Enum for the modes on the dial, corresponds directly to dial index pos. */ -enum class DialMode_e -{ - /* No torque vectoring */ - MODE_0, - /* Normal force torque vectoring */ - MODE_1, - /* PID torque vectoring */ - MODE_2, - /* Launch Ctrl */ - MODE_3, - MODE_4, - MODE_5, -}; /* Enum for defined LED colors. ON will be LED's default color on dashboard*/ enum class LEDColors_e @@ -82,8 +56,8 @@ struct DashComponentInterface_s { /* READ DATA */ // enum for dial position read by controller mux - DialMode_e dial_mode; - DialMode_e cur_dial_mode; + ControllerMode_e dial_mode; + ControllerMode_e cur_dial_mode; // Buttons struct for better naming DashButtons_s button; bool ssok; // safety system OK (IMD?) RENAME @@ -145,13 +119,13 @@ class DashboardInterface float min_cell_voltage, AnalogConversion_s glv_voltage, int launch_state, - DialMode_e dial_mode); + ControllerMode_e dial_mode); /*! getter for the dashboard's current dial position (drive profile) - @return returns a DialMode_e enum with the current dial position + @return returns a ControllerMode_e enum with the current dial position */ - DialMode_e getDialMode(); + ControllerMode_e getDialMode(); /* gets whether the safety system is ok (wtf is a safety system - rename this)*/ bool safetySystemOK(); diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index ffa4e469d..d360458d3 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -9,7 +9,7 @@ #include "circular_buffer.h" #include "NativeEthernet.h" #include "MCU_rev15_defs.h" - +#include "InterfaceParams.h" struct ETHInterfaces { diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index c94223e3e..d8008e80c 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -10,7 +10,8 @@ #include "SteeringEncoderInterface.h" #include "hytech.h" #include "InverterInterface.h" -#include "TorqueControllersData.h" +// #include "TorqueControllersData.h" +#include "SharedDataTypes.h" using InvInt_t = InverterInterface; const int FIXED_POINT_PRECISION = 10000; diff --git a/lib/interfaces/include/VectornavInterface.h b/lib/interfaces/include/VectornavInterface.h index edf525cd9..08a96ed53 100644 --- a/lib/interfaces/include/VectornavInterface.h +++ b/lib/interfaces/include/VectornavInterface.h @@ -3,24 +3,24 @@ #include "FlexCAN_T4.h" #include "hytech.h" -struct vector_nav { - float velocity_x; - float velocity_y; - float velocity_z; - float linear_accel_x; - float linear_accel_y; - float linear_accel_z; - float uncompLinear_accel[3]; // 3D uncompensated linear acceleration - float yaw; - float pitch; - float roll; - double latitude; - double longitude; - double ecef_coords[3]; // x,y,z - uint64_t gps_time; // gps time - uint8_t vn_status; // status - xyz_vec angular_rates; - }; +// struct vectornav { +// float velocity_x; +// float velocity_y; +// float velocity_z; +// float linear_accel_x; +// float linear_accel_y; +// float linear_accel_z; +// float uncompLinear_accel[3]; // 3D uncompensated linear acceleration +// float yaw; +// float pitch; +// float roll; +// double latitude; +// double longitude; +// double ecef_coords[3]; // x,y,z +// uint64_t gps_time; // gps time +// uint8_t vn_status; // status +// xyz_vec angular_rates; +// }; template class VNInterface @@ -29,7 +29,7 @@ class VNInterface /* Watchdog last kicked time */ message_queue *msg_queue_; uint32_t can_id_; - vector_nav vn_data; + vectornav vn_data; public: VNInterface(message_queue *msg_output_queue) { @@ -51,7 +51,7 @@ class VNInterface void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg); void receive_ang_rates_CAN(CAN_message_t &recvd_msg); // getters - vector_nav get_vn_struct(); + vectornav get_vn_struct(); uint32_t get_id() { return can_id_;}; }; diff --git a/lib/interfaces/include/VectornavInterface.tpp b/lib/interfaces/include/VectornavInterface.tpp index be07a4e08..011791ccf 100644 --- a/lib/interfaces/include/VectornavInterface.tpp +++ b/lib/interfaces/include/VectornavInterface.tpp @@ -90,9 +90,9 @@ void VNInterface::receive_ang_rates_CAN(CAN_message_t & recvd_msg * @brief * getter method for returning vn_data structure * @tparam message_queue - * @return vector_nav + * @return vectornav */ template -vector_nav VNInterface::get_vn_struct() { +vectornav VNInterface::get_vn_struct() { return vn_data; } diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index de22b7c1b..c0ec256c6 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -6,7 +6,7 @@ void DashboardInterface::read(const CAN_message_t &can_msg) DASHBOARD_STATE_t dash_state; Unpack_DASHBOARD_STATE_hytech(&dash_state, can_msg.buf, can_msg.len); - _data.dial_mode = static_cast(dash_state.dial_state); + _data.dial_mode = static_cast(dash_state.dial_state); _data.ssok = dash_state.ssok_above_threshold; _data.shutdown = dash_state.shutdown_h_above_threshold; @@ -77,7 +77,7 @@ void DashboardInterface::tick10(MCUInterface* mcu, float min_cell_voltage, AnalogConversion_s glv_voltage, int launch_state, - DialMode_e dial_mode) + ControllerMode_e dial_mode) { _data.cur_dial_mode = dial_mode; @@ -128,7 +128,7 @@ void DashboardInterface::tick10(MCUInterface* mcu, write(); } -DialMode_e DashboardInterface::getDialMode() {return _data.dial_mode;} +ControllerMode_e DashboardInterface::getDialMode() {return _data.dial_mode;} bool DashboardInterface::startButtonPressed() {return _data.button.start;} bool DashboardInterface::specialButtonPressed() {return _data.button.mark;} diff --git a/lib/mock_interfaces/DashboardInterface.h b/lib/mock_interfaces/DashboardInterface.h index 67688fbec..46be692fa 100644 --- a/lib/mock_interfaces/DashboardInterface.h +++ b/lib/mock_interfaces/DashboardInterface.h @@ -1,33 +1,15 @@ #ifndef DASHBOARDINTERFACE #define DASHBOARDINTERFACE +#include "SharedDataTypes.h" /* Enum for the car's torque limits MOVE ME! - ideally into a TorqueControllerDefs.h file to prevent circular dependencies */ -enum class TorqueLimit_e -{ - TCMUX_LOW_TORQUE = 0, - TCMUX_MID_TORQUE = 1, - TCMUX_FULL_TORQUE = 2, - TCMUX_NUM_TORQUE_LIMITS = 3, -}; -/* Enum for the modes on the dial, corresponds directly to dial index pos. */ -enum class DialMode_e -{ - /* No torque vectoring */ - MODE_0, - /* Normal force torque vectoring */ - MODE_1, - /* PID torque vectoring */ - MODE_2, - /* Launch Ctrl */ - MODE_3, - MODE_4, - MODE_5, -}; + + /* Enum for defined LED colors. ON will be LED's default color on dashboard*/ enum class LEDColors_e diff --git a/lib/mock_interfaces/VectornavInterface.h b/lib/mock_interfaces/VectornavInterface.h index 05c486d6e..7c2289e19 100644 --- a/lib/mock_interfaces/VectornavInterface.h +++ b/lib/mock_interfaces/VectornavInterface.h @@ -1,24 +1,7 @@ #ifndef VECTORNAVINTERFACE #define VECTORNAVINTERFACE +#include "SharedDataTypes.h" -struct vector_nav { - float velocity_x; - float velocity_y; - float velocity_z; - float linear_accel_x; - float linear_accel_y; - float linear_accel_z; - float uncompLinear_accel[3]; // 3D uncompensated linear acceleration - float yaw; - float pitch; - float roll; - double latitude; - double longitude; - double ecef_coords[3]; // x,y,z - uint64_t gps_time; // gps time - uint8_t vn_status; // status - xyz_vec angular_rates; - }; template class VNInterface @@ -27,7 +10,7 @@ class VNInterface /* Watchdog last kicked time */ message_queue *msg_queue_; uint32_t can_id_; - vector_nav vn_data; + vectornav vn_data; public: VNInterface(message_queue *msg_output_queue) { @@ -36,7 +19,7 @@ class VNInterface /* Kick watchdog */ // getters - vector_nav get_vn_struct(); + vectornav get_vn_struct(); uint32_t get_id() { return can_id_;}; }; diff --git a/lib/systems/include/PhysicalParameters.h b/lib/shared_data/include/PhysicalParameters.h similarity index 100% rename from lib/systems/include/PhysicalParameters.h rename to lib/shared_data/include/PhysicalParameters.h diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h new file mode 100644 index 000000000..73ecd9dd4 --- /dev/null +++ b/lib/shared_data/include/SharedDataTypes.h @@ -0,0 +1,115 @@ +#ifndef TORQUECONTROLLERSDATA +#define TORQUECONTROLLERSDATA +#include +#include "Utility.h" + +struct PIDTVTorqueControllerData +{ + float controller_input; + float controller_output; + float fl_torque_delta; + float fr_torque_delta; + float rl_torque_delta; + float rr_torque_delta; +}; +enum class TorqueLimit_e +{ + TCMUX_LOW_TORQUE = 0, + TCMUX_MID_TORQUE = 1, + TCMUX_FULL_TORQUE = 2, + TCMUX_NUM_TORQUE_LIMITS = 3, +}; + +/* Enum for the modes on the dial, corresponds directly to dial index pos. */ +enum class ControllerMode_e +{ + /* No torque vectoring */ + MODE_0, + /* Normal force torque vectoring */ + MODE_1, + /* PID torque vectoring */ + MODE_2, + /* Launch Ctrl */ + MODE_3, + MODE_4, + MODE_5, +}; + +struct PedalsSystemData_s +{ + bool accelImplausible : 1; + bool brakeImplausible : 1; + bool brakePressed : 1; + bool accelPressed : 1; + bool mechBrakeActive : 1; + bool brakeAndAccelPressedImplausibility : 1; + bool implausibilityExceededMaxDuration : 1; + float accelPercent; + float brakePercent; + float regenPercent; +}; +struct DrivetrainDynamicReport_s +{ + uint16_t measuredInverterFLPackVoltage; + float measuredSpeeds[NUM_MOTORS]; + float measuredTorques[NUM_MOTORS]; + float measuredTorqueCurrents[NUM_MOTORS]; + float measuredMagnetizingCurrents[NUM_MOTORS]; +}; +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 +}; + +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}}; + +struct TorqueControllerOutput_s +{ + DrivetrainCommand_s command; + bool ready; +}; +struct vectornav +{ + float velocity_x; + float velocity_y; + float velocity_z; + float linear_accel_x; + float linear_accel_y; + float linear_accel_z; + float uncompLinear_accel[3]; // 3D uncompensated linear acceleration + float yaw; + float pitch; + float roll; + double latitude; + double longitude; + double ecef_coords[3]; // x,y,z + uint64_t gps_time; // gps time + uint8_t vn_status; // status + xyz_vec angular_rates; +}; + +enum class TorqueControllerMuxError +{ + NO_ERROR = 0, + ERROR_SPEED_DIFF_TOO_HIGH = 1, + ERROR_TORQUE_DIFF_TOO_HIGH = 2 +}; + +struct TorqueControllerMuxStatus +{ + TorqueControllerMuxError current_error; + ControllerMode_e current_controller_mode_; + bool output_is_bypassing_limits; +}; +struct car_state +{ + // data + DrivetrainDynamicReport_s drivetrain_data; + PedalsSystemData_s pedals_data; + vectornav vn_data; +}; + +#endif \ No newline at end of file diff --git a/lib/shared_data/include/TorqueControllersData.h b/lib/shared_data/include/TorqueControllersData.h deleted file mode 100644 index f54d510a3..000000000 --- a/lib/shared_data/include/TorqueControllersData.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef TORQUECONTROLLERSDATA -#define TORQUECONTROLLERSDATA - -struct PIDTVTorqueControllerData -{ - float controller_input; - float controller_output; - float fl_torque_delta; - float fr_torque_delta; - float rl_torque_delta; - float rr_torque_delta; -}; - -#endif \ No newline at end of file diff --git a/lib/systems/include/Utility.h b/lib/shared_data/include/Utility.h similarity index 95% rename from lib/systems/include/Utility.h rename to lib/shared_data/include/Utility.h index dc2a9d508..820d0f4a4 100644 --- a/lib/systems/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -1,6 +1,6 @@ #ifndef __UTILITY_H__ #define __UTILITY_H__ - +#include "PhysicalParameters.h" // Defines const int FL = 0; const int FR = 1; diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 29e796fb4..8c8ad87a5 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -109,7 +109,7 @@ class CASESystem /// @return controller output DrivetrainCommand_s evaluate( const SysTick_s &tick, - const vector_nav &vn_data, + const vectornav &vn_data, const SteeringSystemData_s &steering_data, const DrivetrainDynamicReport_s &drivetrain_data, const veh_vec &load_cell_vals, diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 4d0ff6ba2..cdd85b7e5 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -3,7 +3,7 @@ template DrivetrainCommand_s CASESystem::evaluate( const SysTick_s &tick, - const vector_nav &vn_data, + const vectornav &vn_data, const SteeringSystemData_s &steering_data, const DrivetrainDynamicReport_s &drivetrain_data, const veh_vec &load_cell_vals, diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 7a2c05640..7afb108ca 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -6,21 +6,8 @@ #include "stdint.h" #include "SysClock.h" #include "MCUInterface.h" +#include "SharedDataTypes.h" -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 -}; - -struct DrivetrainDynamicReport_s -{ - uint16_t measuredInverterFLPackVoltage; - float measuredSpeeds[NUM_MOTORS]; - float measuredTorques[NUM_MOTORS]; - float measuredTorqueCurrents[NUM_MOTORS]; - float measuredMagnetizingCurrents[NUM_MOTORS]; -}; template class DrivetrainSystem diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index a47acd821..f5af8e5e1 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -3,23 +3,11 @@ #include #include #include "AnalogSensorsInterface.h" - +#include "SharedDataTypes.h" #include "SysClock.h" /// @brief system interface struct that contains the data from the pedal system -struct PedalsSystemData_s -{ - bool accelImplausible : 1; - bool brakeImplausible : 1; - bool brakePressed : 1; - bool accelPressed : 1; - bool mechBrakeActive : 1; - bool brakeAndAccelPressedImplausibility : 1; - bool implausibilityExceededMaxDuration : 1; - float accelPercent; - float brakePercent; - float regenPercent; -}; + /// @brief Pedals params struct that will hold min / max that will be used for evaluateion. struct PedalsParams diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index cd8dc0501..79e00e1c5 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -27,13 +27,13 @@ class TorqueControllerMux 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_5, TorqueController_e::TC_NO_CONTROLLER}, + std::unordered_map dialModeMap_ = { + {ControllerMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, + {ControllerMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING}, + {ControllerMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM}, + {ControllerMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH}, + {ControllerMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH}, + {ControllerMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, }; std::unordered_map torqueLimitMap_ = { {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0}, @@ -42,7 +42,7 @@ class TorqueControllerMux }; TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; - DialMode_e cur_dial_mode_ = DialMode_e::MODE_0; + ControllerMode_e cur_dial_mode_ = ControllerMode_e::MODE_0; TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; @@ -89,9 +89,9 @@ class TorqueControllerMux const PedalsSystemData_s &pedalsData, const SteeringSystemData_s &steeringData, const LoadCellInterfaceOutput_s &loadCellData, - DialMode_e dashboardDialMode, + ControllerMode_e dashboardDialMode, bool dashboardTorqueModeButtonPressed, - const vector_nav &vn_data, + const vectornav &vn_data, const DrivetrainCommand_s &CASECommand ); const DrivetrainCommand_s &getDrivetrainCommand() @@ -119,7 +119,7 @@ class TorqueControllerMux - const DialMode_e getDialMode() + const ControllerMode_e getDialMode() { return cur_dial_mode_; } diff --git a/lib/systems/include/TorqueControllerMuxV2.h b/lib/systems/include/TorqueControllerMuxV2.h new file mode 100644 index 000000000..adec9a50c --- /dev/null +++ b/lib/systems/include/TorqueControllerMuxV2.h @@ -0,0 +1,79 @@ +#ifndef TORQUECONTROLLERMUX +#define TORQUECONTROLLERMUX + +#include +// #include "TorqueControllers.h" +#include "SharedDataTypes.h" + +// notes: +// 21 torque limit should be first + +// tc mux needs to handle these things: +// 1 swapping between controller outputs +// 2 turning on and off running of controllers +// 3 application of safeties and limits to controller outputs +// 4 torque limit changing (torque mode) --> +// TODO the torque limit value changing should be handled in the dashboard interface + +namespace TC_MUX_DEFAULT_PARAMS +{ + const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s + const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm +} + +class Controller +{ +private: + // common function for calculation of the limited torque request that goes into controllers + float get_torque_request_(float torque_limit_nm, const PedalsSystemData_s &pedals_data) { return 0; }; + +public: + virtual TorqueControllerOutput_s evaluate(const car_state &state) = 0; +}; + +// namespace TC_MUX +// { +template +class TorqueControllerMuxv2 +{ + static_assert(num_controllers >= 1, "Must create TC mux with at least 1 controller"); + +private: + std::array controller_pointers_; + std::array mux_bypass_limits_; + + std::unordered_map torque_limit_map_ = { + {TorqueLimit_e::TCMUX_LOW_TORQUE, AMK_MAX_TORQUE}, + {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, + {TorqueLimit_e::TCMUX_FULL_TORQUE, 10.0}}; + float max_change_speed_, max_torque_pos_change_delta_; + DrivetrainCommand_s prev_command_ = {}; + TorqueControllerMuxStatus current_status_ = {}; + TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + DrivetrainCommand_s previous_controller_command, + DrivetrainCommand_s desired_controller_out); + DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); + DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); + DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); + DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); +public: + TorqueControllerMuxv2(std::array controller_pointers, + std::array mux_bypass_limits, + float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, + float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE) : controller_pointers_(controller_pointers), + mux_bypass_limits_(mux_bypass_limits), + max_change_speed_(max_change_speed), + max_torque_pos_change_delta_(max_torque_pos_change_delta_) + + { + } + + const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } + + DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e controller_command_torque_limit, + const car_state &input_state); +}; +// } +#include "TorqueControllerMuxV2.tpp" +#endif // __TORQUECONTROLLERMUXV2_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMuxV2.tpp b/lib/systems/include/TorqueControllerMuxV2.tpp new file mode 100644 index 000000000..fb5755489 --- /dev/null +++ b/lib/systems/include/TorqueControllerMuxV2.tpp @@ -0,0 +1,168 @@ +#include "TorqueControllerMuxV2.h" + +template +DrivetrainCommand_s TorqueControllerMuxv2::getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e requested_torque_limit, + const car_state &input_state) +{ + TorqueControllerOutput_s current_output; + + current_output = controller_pointers_[static_cast(current_status_.current_controller_mode_)]->evaluate(input_state); + + bool requesting_controller_change = requested_controller_type != current_status_.current_controller_mode_; + + if (requesting_controller_change) + { + TorqueControllerOutput_s proposed_output = controller_pointers_[static_cast(requested_controller_type)]->evaluate(input_state); + TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); + std::cout << "error state " << static_cast(error_state) < +TorqueControllerMuxError TorqueControllerMuxv2::can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + DrivetrainCommand_s previous_controller_command, + DrivetrainCommand_s desired_controller_out) +{ + bool speedPreventsModeChange = false; + // Check if torque delta permits mode change + 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_); + // 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_; + if (speedPreventsModeChange) + { + return TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH; + } + if (torqueDeltaPreventsModeChange) + { + return TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH; + } + } + return TorqueControllerMuxError::NO_ERROR; +} + +/* Apply limit such that wheelspeed never goes negative */ +template +DrivetrainCommand_s TorqueControllerMuxv2::apply_positive_speed_limit_(const DrivetrainCommand_s &command) +{ + DrivetrainCommand_s out; + out = command; + for (int i = 0; i < NUM_MOTORS; i++) + { + out.speeds_rpm[i] = std::max(0.0f, command.speeds_rpm[i]); + } + return out; +} + +template +DrivetrainCommand_s TorqueControllerMuxv2::apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque) +{ + DrivetrainCommand_s out = command; + float avg_torque = 0; + // get the average torque accross all 4 wheels + for (int i = 0; i < NUM_MOTORS; i++) + { + + avg_torque += abs(command.torqueSetpoints[i]); + } + avg_torque /= NUM_MOTORS; + + // if this is greather than the torque limit, scale down + if (avg_torque > max_torque) + { + // get the scale of avg torque above max torque + float scale = avg_torque / max_torque; + // divide by scale to lower avg below max torque + for (int i = 0; i < NUM_MOTORS; i++) + { + out.torqueSetpoints[i] = out.torqueSetpoints[i] / scale; + } + } + return out; +} + +/* + Apply power limit 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 +*/ +template +DrivetrainCommand_s TorqueControllerMuxv2::apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque) +{ + DrivetrainCommand_s out = command; + float net_torque_mag = 0; + float net_power = 0; + + // calculate current mechanical power + for (int i = 0; i < NUM_MOTORS; i++) + { + // get the total magnitude of torque from all 4 wheels + // sum up net torque + net_torque_mag += abs(out.torqueSetpoints[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)); + } + + // only evaluate power limit if current power exceeds it + if (net_power > (power_limit)) + { + for (int i = 0; i < NUM_MOTORS; i++) + { + + // TODO: SOMEBODY PLZ MAKE THIS WORK + // 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); + // 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); + + // power / omega (motor rad/s) to get torque per wheel + out.torqueSetpoints[i] = abs(power_per_corner / (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + out.torqueSetpoints[i] = std::max(0.0f, std::min(command.torqueSetpoints[i], max_torque)); + } + } + return out; +} + +template +DrivetrainCommand_s TorqueControllerMuxv2::apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data) +{ + DrivetrainCommand_s out = command; + const float noRegenLimitKPH = 10.0; + const float fullRegenLimitKPH = 5.0; + float maxWheelSpeed = 0.0; + float torqueScaleDown = 0.0; + bool allWheelsRegen = true; // true when all wheels are targeting speeds below the current wheel speed + + for (int i = 0; i < NUM_MOTORS; i++) + { + maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain_data.measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); + + allWheelsRegen &= (command.speeds_rpm[i] < abs(drivetrain_data.measuredSpeeds[i]) || command.speeds_rpm[i] == 0); + } + + // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH + // linearly interpolate the scale factor between noRegenLimitKPH and fullRegenLimitKPH + torqueScaleDown = std::min(1.0f, std::max(0.0f, (maxWheelSpeed - fullRegenLimitKPH) / (noRegenLimitKPH - fullRegenLimitKPH))); + + if (allWheelsRegen) + { + for (int i = 0; i < NUM_MOTORS; i++) + { + out.torqueSetpoints[i] *= torqueScaleDown; + } + } + return out; +} \ No newline at end of file diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 18ed91e28..714faa562 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -14,8 +14,7 @@ #include "accel_lookup.h" -#include "TorqueControllersData.h" - +#include "SharedDataTypes.h" /* CONTROLLER CONSTANTS */ @@ -49,15 +48,9 @@ const float launch_stop_accel_threshold = .5; /* DRIVETRAIN STRUCTS */ -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}}; -struct TorqueControllerOutput_s -{ - DrivetrainCommand_s command; - bool ready; -}; + + /* TC STRUCTS */ struct TCCaseWrapperTick_s @@ -238,9 +231,9 @@ class BaseLaunchController void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], - const vector_nav *vn_data); + const vectornav *vn_data); - virtual void calc_launch_algo(const vector_nav *vn_data) = 0; + virtual void calc_launch_algo(const vectornav *vn_data) = 0; }; class TorqueControllerSimpleLaunch : public TorqueController, public BaseLaunchController @@ -264,7 +257,7 @@ class TorqueControllerSimpleLaunch : public TorqueController, LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vector_nav *vn_data) override; + void calc_launch_algo(const vectornav *vn_data) override; }; class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController @@ -289,7 +282,7 @@ class TorqueControllerSlipLaunch : public TorqueController, publ LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vector_nav *vn_data) override; + void calc_launch_algo(const vectornav *vn_data) override; }; class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController @@ -313,7 +306,7 @@ class TorqueControllerLookupLaunch : public TorqueController, LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vector_nav *vn_data) override; + void calc_launch_algo(const vectornav *vn_data) override; }; class TorqueControllerCASEWrapper : public TorqueController diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index fe39c1e27..0d4e28b58 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -8,9 +8,9 @@ void TorqueControllerMux::tick( const PedalsSystemData_s &pedalsData, const SteeringSystemData_s &steeringData, const LoadCellInterfaceOutput_s &loadCellData, - DialMode_e dashboardDialMode, + ControllerMode_e dashboardDialMode, bool dashboardTorqueModeButtonPressed, - const vector_nav &vn_data, + const vectornav &vn_data, const DrivetrainCommand_s &CASECommand) { // Tick all torque controllers diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 4351c4dc8..d12adeb2e 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -152,7 +152,7 @@ void BaseLaunchController::tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], - const vector_nav* vn_data) + const vectornav* vn_data) { if (tick.triggers.trigger100){ @@ -256,7 +256,7 @@ void BaseLaunchController::tick( } } -void TorqueControllerSimpleLaunch::calc_launch_algo(const vector_nav* vn_data) { +void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav* vn_data) { /* Stolen launch algo from HT07. This ramps up the speed target over time. launch rate target is m/s^2 and is the target acceleration rate @@ -270,7 +270,7 @@ 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) { +void TorqueControllerSlipLaunch::calc_launch_algo(const vectornav* 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 @@ -291,7 +291,7 @@ void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); } -void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { +void TorqueControllerLookupLaunch::calc_launch_algo(const vectornav* vn_data) { launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); diff --git a/platformio.ini b/platformio.ini index b1b24ee6b..c83ff77a2 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,3 +1,10 @@ +[common] +lib_deps_shared = + Nanopb + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v45 + https://github.com/hytech-racing/shared_firmware_systems.git + https://github.com/hytech-racing/HT_params/releases/download/2024-05-05T13_33_07/ht_eth_pb_lib.tar.gz + [env:test_env] platform = native test_framework = googletest @@ -11,10 +18,8 @@ lib_ignore = test_ignore= test_interfaces* lib_deps= - git+ssh://git@github.com/hytech-racing/CASE_lib.git#v45 - - https://github.com/hytech-racing/shared_firmware_systems.git - + ${common.lib_deps_shared} + [env:teensy41] ; Testing variables test_framework=unity @@ -46,18 +51,14 @@ framework = arduino upload_protocol = teensy-cli extra_scripts = pre:extra_script.py lib_deps = - SPI - Nanopb - https://github.com/vjmuzik/NativeEthernet.git - https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz - https://github.com/hytech-racing/shared_firmware_interfaces.git#native_adcs - https://github.com/hytech-racing/shared_firmware_systems.git - https://github.com/RCMast3r/spi_libs - https://github.com/tonton81/FlexCAN_T4 - https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/86/can_lib.tar.gz - git+ssh://git@github.com/hytech-racing/CASE_lib.git#v45 - + ${common.lib_deps_shared} + SPI + https://github.com/hytech-racing/shared_firmware_interfaces.git + https://github.com/tonton81/FlexCAN_T4 + https://github.com/ssilverman/QNEthernet#v0.26.0 + https://github.com/RCMast3r/hytech_can#testing_new_inv_ids + https://github.com/hytech-racing/HT_CAN/releases/download/86/can_lib.tar.gz + [env:test_can_on_teensy] lib_ignore = mock_interfaces diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 6a3061db6..12876f0ac 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -2,10 +2,12 @@ #include "state_machine_test.h" #include "pedals_system_test.h" -#include "torque_controller_mux_test.h" +// #include "torque_controller_mux_test.h" +#include "tc_mux_v2_testing.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" #include "test_CASE.h" + // #include "param_system_test.h" int main(int argc, char **argv) diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h new file mode 100644 index 000000000..745259cbf --- /dev/null +++ b/test/test_systems/tc_mux_v2_testing.h @@ -0,0 +1,54 @@ +#ifndef __TC_MUX_V2_TESTING_H__ +#define __TC_MUX_V2_TESTING_H__ + +#include "TorqueControllerMuxV2.h" + +template +void set_outputs(controller_type &controller, float mps, float torque) +{ + controller.output.command.speeds_rpm[0] = METERS_PER_SECOND_TO_RPM * mps; + 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; +} + +class TestControllerType : public virtual Controller +{ + +public: + TorqueControllerOutput_s output; + TorqueControllerOutput_s evaluate(const car_state &state) override { return output; } + // void update_input_state(const car_state &state) override { } +}; + +TEST(TorqueControllerMuxTesting, test_construction) +{ + TestControllerType inst1, inst2; + TorqueControllerMuxv2<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); +} + +// ensure that swapping to a controller that has a higher desired output speed than previously +// commanded that we dont switch +TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) +{ + TestControllerType inst1, inst2; + set_outputs(inst1, 0, 1); + set_outputs(inst2, 6, 1); + TorqueControllerMuxv2<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + car_state state = {.drivetrain_data = {6 * METERS_PER_SECOND_TO_RPM, 6 * METERS_PER_SECOND_TO_RPM, 6 * METERS_PER_SECOND_TO_RPM, 6 * METERS_PER_SECOND_TO_RPM}, .pedals_data = {}, .vn_data = {}}; + + auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + + // set_outputs(inst2, ) + // 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); +} + +#endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index 7d6a0e4c2..0ab60785d 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -47,7 +47,7 @@ // (const AnalogConversion_s) {}, // (const AnalogConversion_s) {}, // (const AnalogConversion_s) {}, -// DialMode_e::MODE_1, +// ControllerMode_e::MODE_1, // buttonTimeSeries[i] // ); // // Test conditions @@ -70,7 +70,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) TorqueControllerMux torque_controller_mux = TorqueControllerMux(); DrivetrainCommand_s resulting_torque_command; - vector_nav vn_data{}; + vectornav vn_data{}; DrivetrainDynamicReport_s simulated_drivetrain_dynamics = { .measuredInverterFLPackVoltage = 550, @@ -108,7 +108,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -129,7 +129,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -142,7 +142,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -164,7 +164,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) cur_tick = clock.tick(0); TorqueControllerMux torque_controller_mux = TorqueControllerMux(); DrivetrainCommand_s resulting_torque_command; - vector_nav vn_data{}; + vectornav vn_data{}; DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { .measuredInverterFLPackVoltage = 550, @@ -220,7 +220,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -241,7 +241,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -254,7 +254,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -347,7 +347,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { TorqueControllerMux torque_controller_mux = TorqueControllerMux(); DrivetrainCommand_s resulting_torque_command; - vector_nav vn_data{}; + vectornav vn_data{}; DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { .measuredInverterFLPackVoltage = 550, @@ -427,7 +427,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -440,7 +440,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -453,7 +453,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -470,7 +470,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -487,7 +487,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -504,7 +504,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -520,7 +520,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -537,7 +537,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { simulated_accel_and_brake_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, + ControllerMode_e::MODE_3, false, vn_data, (const DrivetrainCommand_s) {} @@ -555,7 +555,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { TorqueControllerMux torque_controller_mux = TorqueControllerMux(); DrivetrainCommand_s resulting_torque_command; - vector_nav vn_data{}; + vectornav vn_data{}; DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { .measuredInverterFLPackVoltage = 550, @@ -635,7 +635,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, + ControllerMode_e::MODE_0, false, vn_data, (const DrivetrainCommand_s) {} @@ -648,7 +648,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} @@ -661,7 +661,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_no_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} @@ -678,7 +678,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} @@ -691,7 +691,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} @@ -710,7 +710,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} @@ -732,7 +732,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} @@ -752,7 +752,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { simulated_full_accel_press, (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, + ControllerMode_e::MODE_4, false, vn_data, (const DrivetrainCommand_s) {} From bd1d066575e994ea7266446535981dd0b2ce652e Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 10 May 2024 01:26:30 -0400 Subject: [PATCH 02/31] in progress --- lib/shared_data/include/SharedDataTypes.h | 2 + lib/systems/include/BaseController.h | 15 +++ .../include/LoadCellVectoringController.h | 71 +++++++++++++ lib/systems/include/SimpleController.h | 35 ++++++ lib/systems/include/TorqueControllerMuxV2.h | 74 +++++++++---- lib/systems/include/TorqueControllerMuxV2.tpp | 10 +- .../src/LoadCellVectoringController.cpp | 100 ++++++++++++++++++ lib/systems/src/SimpleController.cpp | 55 ++++++++++ test/test_systems/tc_mux_v2_testing.h | 31 +++++- 9 files changed, 370 insertions(+), 23 deletions(-) create mode 100644 lib/systems/include/BaseController.h create mode 100644 lib/systems/include/LoadCellVectoringController.h create mode 100644 lib/systems/include/SimpleController.h create mode 100644 lib/systems/src/LoadCellVectoringController.cpp create mode 100644 lib/systems/src/SimpleController.cpp diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 73ecd9dd4..a7d2123ac 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -107,7 +107,9 @@ struct TorqueControllerMuxStatus struct car_state { // data + SysTick_s systick; DrivetrainDynamicReport_s drivetrain_data; + // GPS data; PedalsSystemData_s pedals_data; vectornav vn_data; }; diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h new file mode 100644 index 000000000..2f9d70c69 --- /dev/null +++ b/lib/systems/include/BaseController.h @@ -0,0 +1,15 @@ +#ifndef BASECONTROLLER +#define BASECONTROLLER +#include "SharedDataTypes.h" + +class Controller +{ +private: + // common function for calculation of the limited torque request that goes into controllers + float get_torque_request_(float torque_limit_nm, const PedalsSystemData_s &pedals_data) { return 0; }; + +public: + virtual TorqueControllerOutput_s evaluate(const car_state &state) = 0; +}; + +#endif \ No newline at end of file diff --git a/lib/systems/include/LoadCellVectoringController.h b/lib/systems/include/LoadCellVectoringController.h new file mode 100644 index 000000000..07fb1995c --- /dev/null +++ b/lib/systems/include/LoadCellVectoringController.h @@ -0,0 +1,71 @@ +#ifndef LOADCELLVECTORINGCONTROLLER +#define LOADCELLVECTORINGCONTROLLER + +#include "BaseController.h" +class TorqueControllerLoadCellVectoring : public Controller +{ +private: + TorqueControllerOutput_s &writeout_; + float frontTorqueScale_ = 1.0; + float rearTorqueScale_ = 1.0; + float frontRegenTorqueScale_ = 1.0; + float rearRegenTorqueScale_ = 1.0; + /* + FIR filter designed with + http://t-filter.appspot.com + + sampling frequency: 100 Hz + + * 0 Hz - 10 Hz + gain = 1 + desired ripple = 5 dB + actual ripple = 1.7659949026015025 dB + + * 40 Hz - 50 Hz + gain = 0 + desired attenuation = -40 dB + actual attenuation = -47.34009380570117 dB + */ + const static int numFIRTaps_ = 5; + float FIRTaps_[numFIRTaps_] = { + 0.07022690881526232, + 0.27638313122745306, + 0.408090001549378, + 0.27638313122745306, + 0.07022690881526232}; + int FIRCircBufferHead = 0; // index of the latest sample in the raw buffer + float loadCellForcesRaw_[4][numFIRTaps_] = {}; + float loadCellForcesFiltered_[4] = {}; + // Some checks that can disable the controller + const int errorCountThreshold_ = 25; + int loadCellsErrorCounter_[4] = {}; + bool FIRSaturated_ = false; + bool ready_ = false; + +public: + /// @brief load cell TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance + /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command + /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD + /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear + TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) + : writeout_(writeout), + frontTorqueScale_(2.0 - rearTorqueScale), + rearTorqueScale_(rearTorqueScale), + frontRegenTorqueScale_(2.0 - regenTorqueScale), + rearRegenTorqueScale_(regenTorqueScale) + { + writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.ready = false; + } + TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0, 1.0) {} + + void tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + float torqueLimit, + const LoadCellInterfaceOutput_s &loadCellData + ); + TorqueControllerOutput_s evaluate(const car_state &state); +}; + +#endif \ No newline at end of file diff --git a/lib/systems/include/SimpleController.h b/lib/systems/include/SimpleController.h new file mode 100644 index 000000000..59c72cf56 --- /dev/null +++ b/lib/systems/include/SimpleController.h @@ -0,0 +1,35 @@ +#ifndef SIMPLECONTROLLER +#define SIMPLECONTROLLER + +#include "BaseController.h" + +class SimpleController : public Controller +{ +private: + TorqueControllerOutput_s &writeout_; + float frontTorqueScale_ = 1.0; + float rearTorqueScale_ = 1.0; + float frontRegenTorqueScale_ = 1.0; + float rearRegenTorqueScale_ = 1.0; + +public: + /// @brief simple TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance + /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command + /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD + /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear + TorqueControllerSimple(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) + : writeout_(writeout), + frontTorqueScale_(2.0 - rearTorqueScale), + rearTorqueScale_(rearTorqueScale), + frontRegenTorqueScale_(2.0 - regenTorqueScale), + rearRegenTorqueScale_(regenTorqueScale) + { + writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.ready = true; + } + + void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData); + TorqueControllerOutput_s evaluate(const car_state &state); +}; + +#endif \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMuxV2.h b/lib/systems/include/TorqueControllerMuxV2.h index adec9a50c..b6863c667 100644 --- a/lib/systems/include/TorqueControllerMuxV2.h +++ b/lib/systems/include/TorqueControllerMuxV2.h @@ -2,8 +2,8 @@ #define TORQUECONTROLLERMUX #include -// #include "TorqueControllers.h" #include "SharedDataTypes.h" +#include "BaseController.h" // notes: // 21 torque limit should be first @@ -15,61 +15,99 @@ // 4 torque limit changing (torque mode) --> // TODO the torque limit value changing should be handled in the dashboard interface + +// TODOs +// - [-] make the controllers inherit from the base controller class +// - [x] port TorqueControllerSimple +// - [ ] port TorqueControllerLoadCellVectoring +// - [ ] port BaseLaunchController +// - [ ] port TorqueControllerSimpleLaunch +// - [ ] port TorqueControllerLookupLaunch +// - [ ] port CASE +// - [ ] integrate into state machine +// - [ ] set the car_state from all sources +// - [ ] get dial and torque mode from the dashboard +// - [ ] write integration test for the real controllers +// - [ ] update the state machine unit test with integration test of new tc mux + + + namespace TC_MUX_DEFAULT_PARAMS { const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm -} - -class Controller -{ -private: - // common function for calculation of the limited torque request that goes into controllers - float get_torque_request_(float torque_limit_nm, const PedalsSystemData_s &pedals_data) { return 0; }; - -public: - virtual TorqueControllerOutput_s evaluate(const car_state &state) = 0; + const float MAX_POWER_LIMIT = 630000.0; }; + // namespace TC_MUX // { + +template +struct check_tc_mux +{ + +}; template class TorqueControllerMuxv2 { - static_assert(num_controllers >= 1, "Must create TC mux with at least 1 controller"); + static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); + + private: std::array controller_pointers_; + std::array mux_bypass_limits_; std::unordered_map torque_limit_map_ = { {TorqueLimit_e::TCMUX_LOW_TORQUE, AMK_MAX_TORQUE}, {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, {TorqueLimit_e::TCMUX_FULL_TORQUE, 10.0}}; - float max_change_speed_, max_torque_pos_change_delta_; + 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, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out); + DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); + public: - TorqueControllerMuxv2(std::array controller_pointers, + + TorqueControllerMuxv2() = delete; + /// @brief constructor for the TC mux + /// @param controller_pointers the array of pointers to the controllers being muxed between + /// @param mux_bypass_limits the array of aligned bools for determining if the limits should be applied to the controller outputs defaults to TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + /// @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 + explicit TorqueControllerMuxv2(std::array controller_pointers, std::array mux_bypass_limits, float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, - float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE) : controller_pointers_(controller_pointers), - mux_bypass_limits_(mux_bypass_limits), - max_change_speed_(max_change_speed), - max_torque_pos_change_delta_(max_torque_pos_change_delta_) + float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE, + float max_power_limit = TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT) : controller_pointers_(controller_pointers), + mux_bypass_limits_(mux_bypass_limits), + max_change_speed_(max_change_speed), + max_torque_pos_change_delta_(max_torque_pos_change_delta), + max_power_limit_(max_power_limit) + { + static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); + } const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } + /// @brief function that evaluates the mux, controllers and gets the current 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 DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e controller_command_torque_limit, const car_state &input_state); diff --git a/lib/systems/include/TorqueControllerMuxV2.tpp b/lib/systems/include/TorqueControllerMuxV2.tpp index fb5755489..bf320dd2c 100644 --- a/lib/systems/include/TorqueControllerMuxV2.tpp +++ b/lib/systems/include/TorqueControllerMuxV2.tpp @@ -15,7 +15,7 @@ DrivetrainCommand_s TorqueControllerMuxv2::getDrivetrainCommand { TorqueControllerOutput_s proposed_output = controller_pointers_[static_cast(requested_controller_type)]->evaluate(input_state); TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); - std::cout << "error state " << static_cast(error_state) <(error_state) << std::endl; if (error_state == TorqueControllerMuxError::NO_ERROR) { current_status_.current_controller_mode_ = requested_controller_type; @@ -23,7 +23,13 @@ DrivetrainCommand_s TorqueControllerMuxv2::getDrivetrainCommand } current_status_.current_error = error_state; } - + if (!mux_bypass_limits_[static_cast(current_status_.current_controller_mode_)]) + { + 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_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); + } return current_output.command; } diff --git a/lib/systems/src/LoadCellVectoringController.cpp b/lib/systems/src/LoadCellVectoringController.cpp new file mode 100644 index 000000000..a8e99e824 --- /dev/null +++ b/lib/systems/src/LoadCellVectoringController.cpp @@ -0,0 +1,100 @@ +#include "LoadCellVectoringController.h" + + +void TorqueControllerLoadCellVectoring::tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + float torqueLimit, + const LoadCellInterfaceOutput_s &loadCellData +) +{ + // Calculate torque commands at 100hz + if (tick.triggers.trigger100) + { + // Apply FIR filter to load cell data + loadCellForcesRaw_[0][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FL; + loadCellForcesRaw_[1][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FR; + loadCellForcesRaw_[2][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RL; + loadCellForcesRaw_[3][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RR; + + for (int i = 0; i < 4; i++) + { + loadCellForcesFiltered_[i] = 0.0f; + for (int FIROffset = 0; FIROffset < numFIRTaps_; FIROffset++) + { + int index = (FIRCircBufferHead + FIROffset) % numFIRTaps_; + loadCellForcesFiltered_[i] += loadCellForcesRaw_[i][index] * FIRTaps_[FIROffset]; + } + } + FIRCircBufferHead = (FIRCircBufferHead + 1) % numFIRTaps_; + if (FIRCircBufferHead == numFIRTaps_ - 1) + FIRSaturated_ = true; + + // Do sanity checks on raw data + loadCellsErrorCounter_[0] = loadCellData.loadCellConversions.FL.raw != 4095 && loadCellData.loadCellConversions.FL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; + loadCellsErrorCounter_[1] = loadCellData.loadCellConversions.FR.raw != 4095 && loadCellData.loadCellConversions.FR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; + loadCellsErrorCounter_[2] = loadCellData.loadCellConversions.RL.raw != 4095 && loadCellData.loadCellConversions.RL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; + loadCellsErrorCounter_[3] = loadCellData.loadCellConversions.RR.raw != 4095 && loadCellData.loadCellConversions.RR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; + ready_ = FIRSaturated_ && loadCellsErrorCounter_[0] < errorCountThreshold_ && loadCellsErrorCounter_[1] < errorCountThreshold_ && loadCellsErrorCounter_[2] < errorCountThreshold_ && loadCellsErrorCounter_[3] < errorCountThreshold_; + + writeout_.ready = ready_; + + if (ready_) + { + // Calculate total normal force + float sumNormalForce = 0.0f; + for (int i = 0; i < 4; i++) + { + sumNormalForce += loadCellForcesFiltered_[i]; + } + + // Both pedals are not pressed and no implausibility has been detected + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torquePool; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + // NOTE: using "torquePool" here instead of torqueRequest for legibility + torquePool = accelRequest * AMK_MAX_TORQUE * 4; + + writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RR] = 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; + } + else + { + // Negative torque request + // No load cell vectoring on regen + torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + 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_; + } + } + else + { + writeout_.command = TC_COMMAND_NO_TORQUE; + } + } +} + +TorqueControllerOutput_s TorqueControllerLoadCellVectoring::evaluate(const car_state &state) +{ + tick(state.systick, state.) +} \ No newline at end of file diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/SimpleController.cpp new file mode 100644 index 000000000..a9151ad6d --- /dev/null +++ b/lib/systems/src/SimpleController.cpp @@ -0,0 +1,55 @@ +#include "SimpleController.h" + +void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData) +{ + + // Calculate torque commands at 100hz + if (tick.triggers.trigger100) + { + // Both pedals are not pressed and no implausibility has been detected + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + torqueRequest = accelRequest * AMK_MAX_TORQUE; + + // writeout_.command.speeds_rpm[FL] = accelRequest * AMK_MAX_RPM; + // writeout_.command.speeds_rpm[FR] = accelRequest * AMK_MAX_RPM;pid_input_ + // writeout_.command.speeds_rpm[RL] = accelRequest * AMK_MAX_RPM; + // writeout_.command.speeds_rpm[RR] = accelRequest * AMK_MAX_RPM; + writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RR] = 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_; + } + else + { + // Negative torque request + torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + 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_; + } + } +} + +TorqueControllerOutput_s TorqueControllerSimple::evaluate(const car_state &state) +{ + tick(state.systick, state.pedals_data); + return writeout_; +} \ No newline at end of file diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 745259cbf..e8e5ab090 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -3,6 +3,15 @@ #include "TorqueControllerMuxV2.h" + +template +void set_four_outputs(quad_array_type& out, float val) +{ + out[0] = val; + out[1] = val; + out[2] = val; + out[3] = val; +} template void set_outputs(controller_type &controller, float mps, float torque) { @@ -31,6 +40,8 @@ TEST(TorqueControllerMuxTesting, test_construction) TorqueControllerMuxv2<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); } +// TEST(TorqueControllerMuxTesting, test_) + // ensure that swapping to a controller that has a higher desired output speed than previously // commanded that we dont switch TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) @@ -39,16 +50,30 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) set_outputs(inst1, 0, 1); set_outputs(inst2, 6, 1); TorqueControllerMuxv2<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - car_state state = {.drivetrain_data = {6 * METERS_PER_SECOND_TO_RPM, 6 * METERS_PER_SECOND_TO_RPM, 6 * METERS_PER_SECOND_TO_RPM, 6 * METERS_PER_SECOND_TO_RPM}, .pedals_data = {}, .vn_data = {}}; - + car_state state; + set_four_outputs(state.drivetrain_data.measuredSpeeds, 10000.0); + + state.pedals_data = {}; + state.vn_data = {}; + auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - // set_outputs(inst2, ) + // 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); + + set_outputs(inst1, 0, 1); + set_outputs(inst2, 0, 1); + set_four_outputs(state.drivetrain_data.measuredSpeeds, 0); + + 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); } +// TEST(TorqueControllerMuxTesting) #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file From 0bef5ff037d1aa61279dd70b50d29e893004fb1d Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 10 May 2024 15:26:11 -0400 Subject: [PATCH 03/31] pushing up as i am headed to shop --- lib/shared_data/include/SharedDataTypes.h | 4 +- lib/systems/include/BaseLaunchController.h | 43 +++++++ lib/systems/include/CASEControllerWrapper.h | 19 +++ .../include/LoadCellVectoringController.h | 16 ++- lib/systems/include/LookupLaunchController.h | 35 ++++++ lib/systems/include/SimpleController.h | 2 +- lib/systems/include/SimpleLaunchController.h | 36 ++++++ lib/systems/include/SlipLaunchController.h | 34 ++++++ lib/systems/include/TorqueControllerMuxV2.h | 15 +-- lib/systems/include/TorqueControllers.h | 14 --- lib/systems/src/BaseLaunchController.cpp | 114 ++++++++++++++++++ lib/systems/src/LaunchControllerAlgos.cpp | 66 ++++++++++ .../src/LoadCellVectoringController.cpp | 4 +- 13 files changed, 366 insertions(+), 36 deletions(-) create mode 100644 lib/systems/include/BaseLaunchController.h create mode 100644 lib/systems/include/CASEControllerWrapper.h create mode 100644 lib/systems/include/LookupLaunchController.h create mode 100644 lib/systems/include/SimpleLaunchController.h create mode 100644 lib/systems/include/SlipLaunchController.h create mode 100644 lib/systems/src/BaseLaunchController.cpp create mode 100644 lib/systems/src/LaunchControllerAlgos.cpp diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index a7d2123ac..9d1468e94 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -104,12 +104,14 @@ struct TorqueControllerMuxStatus ControllerMode_e current_controller_mode_; bool output_is_bypassing_limits; }; + struct car_state { // data SysTick_s systick; + SteeringSystemData_s steering_data; DrivetrainDynamicReport_s drivetrain_data; - // GPS data; + LoadCellInterfaceOutput_s loadcell_data; PedalsSystemData_s pedals_data; vectornav vn_data; }; diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h new file mode 100644 index 000000000..cdd8f80e5 --- /dev/null +++ b/lib/systems/include/BaseLaunchController.h @@ -0,0 +1,43 @@ +#ifndef __BASELAUNCHCONTROLLER_H__ +#define __BASELAUNCHCONTROLLER_H__ +#include "SharedDataTypes.h" +#include "BaseController.h" + +enum class LaunchStates_e +{ + NO_LAUNCH_MODE, + LAUNCH_NOT_READY, + LAUNCH_READY, + LAUNCHING +}; + +class BaseLaunchController : public Controller +{ +protected: + TorqueControllerOutput_s writeout_; + uint32_t time_of_launch_; + double initial_ecef_x_; + double initial_ecef_y_; + double initial_ecef_z_; + LaunchStates_e launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; + uint32_t current_millis_; + float launch_speed_target_ = 0.0; + int16_t init_speed_target_ = 0.0; + +public: + BaseLaunchController(int16_t initial_speed_target) + : init_speed_target_(initial_speed_target) + { + writeout.command = TC_COMMAND_NO_TORQUE; + writeout_.ready = true; + } + + void tick(const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + const float wheel_rpms[], + const vectornav &vn_data); + + virtual void calc_launch_algo(const vectornav &vn_data) = 0; + TorqueControllerOutput_s evaluate(const car_state &state) override; +}; +#endif // __BASELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/CASEControllerWrapper.h b/lib/systems/include/CASEControllerWrapper.h new file mode 100644 index 000000000..bd5ea681f --- /dev/null +++ b/lib/systems/include/CASEControllerWrapper.h @@ -0,0 +1,19 @@ +#ifndef __CASECONTROLLERWRAPPER_H__ +#define __CASECONTROLLERWRAPPER_H__ +#include "BaseController.h" + +#include "CASESystem.h" + +class TorqueControllerCASEWrapper : +{ +public: + + TorqueControllerCASEWrapper(const CASESystem& case_instance) : case_instance_(case_instance) + { + } + TorqueControllerOutput_s evaluate(const car_state &state) override; +private: + const CASESystem& case_instance_; +}; + +#endif // __CASECONTROLLERWRAPPER_H__ \ No newline at end of file diff --git a/lib/systems/include/LoadCellVectoringController.h b/lib/systems/include/LoadCellVectoringController.h index 07fb1995c..84ca1a45c 100644 --- a/lib/systems/include/LoadCellVectoringController.h +++ b/lib/systems/include/LoadCellVectoringController.h @@ -5,11 +5,12 @@ class TorqueControllerLoadCellVectoring : public Controller { private: - TorqueControllerOutput_s &writeout_; float frontTorqueScale_ = 1.0; float rearTorqueScale_ = 1.0; float frontRegenTorqueScale_ = 1.0; float rearRegenTorqueScale_ = 1.0; + + TorqueControllerOutput_s writeout_; /* FIR filter designed with http://t-filter.appspot.com @@ -47,9 +48,8 @@ class TorqueControllerLoadCellVectoring : public Controller /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear - TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), + TorqueControllerLoadCellVectoring(float rearTorqueScale, float regenTorqueScale) + : frontTorqueScale_(2.0 - rearTorqueScale), rearTorqueScale_(rearTorqueScale), frontRegenTorqueScale_(2.0 - regenTorqueScale), rearRegenTorqueScale_(regenTorqueScale) @@ -57,15 +57,13 @@ class TorqueControllerLoadCellVectoring : public Controller writeout_.command = TC_COMMAND_NO_TORQUE; writeout_.ready = false; } - TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0, 1.0) {} + TorqueControllerLoadCellVectoring() : TorqueControllerLoadCellVectoring(1.0, 1.0) {} void tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, - float torqueLimit, - const LoadCellInterfaceOutput_s &loadCellData - ); - TorqueControllerOutput_s evaluate(const car_state &state); + const LoadCellInterfaceOutput_s &loadCellData); + TorqueControllerOutput_s evaluate(const car_state &state) override; }; #endif \ No newline at end of file diff --git a/lib/systems/include/LookupLaunchController.h b/lib/systems/include/LookupLaunchController.h new file mode 100644 index 000000000..89c4ddda7 --- /dev/null +++ b/lib/systems/include/LookupLaunchController.h @@ -0,0 +1,35 @@ +#ifndef __LOOKUPLAUNCHCONTROLLER_H__ +#define __LOOKUPLAUNCHCONTROLLER_H__ +#include "BaseLaunchController.h" +#include "accel_lookup.h" + +namespace LookupLaunchControllerParams +{ + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + +}; +class TorqueControllerLookupLaunch : public 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 initial_speed_target the initial speed commanded to the wheels + */ + TorqueControllerLookupLaunch(int16_t initial_speed_target) + : BaseLaunchController(initial_speed_target) {} + + TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} + + LaunchStates_e get_launch_state() override { return launch_state_; } + + void calc_launch_algo(const vectornav &vn_data) override; +}; + + +#endif // __LOOKUPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/SimpleController.h b/lib/systems/include/SimpleController.h index 59c72cf56..b5a16d641 100644 --- a/lib/systems/include/SimpleController.h +++ b/lib/systems/include/SimpleController.h @@ -29,7 +29,7 @@ class SimpleController : public Controller } void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData); - TorqueControllerOutput_s evaluate(const car_state &state); + TorqueControllerOutput_s evaluate(const car_state &state) override; }; #endif \ No newline at end of file diff --git a/lib/systems/include/SimpleLaunchController.h b/lib/systems/include/SimpleLaunchController.h new file mode 100644 index 000000000..f3de1c177 --- /dev/null +++ b/lib/systems/include/SimpleLaunchController.h @@ -0,0 +1,36 @@ +#ifndef __SIMPLELAUNCHCONTROLLER_H__ +#define __SIMPLELAUNCHCONTROLLER_H__ +#include "BaseLaunchController.h" + +/* LAUNCH CONSTANTS */ +namespace SimpleLaunchControllerParams +{ + const float DEFAULT_LAUNCH_RATE = 11.76; + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + +}; + +class TorqueControllerSimpleLaunch : public BaseLaunchController +{ +private: + float launch_rate_target_; + +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 + @param launch_rate specified launch rate in m/s^2 + @param initial_speed_target the initial speed commanded to the wheels + */ + TorqueControllerSimpleLaunch(float launch_rate, int16_t initial_speed_target) + : BaseLaunchController(initial_speed_target), + launch_rate_target_(launch_rate) {} + + TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SimpleLaunchControllerParams::DEFAULT_LAUNCH_RATE, SimpleLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} + + LaunchStates_e get_launch_state() override { return launch_state_; } + + void calc_launch_algo(const vectornav &vn_data) override; +}; +#endif // __SIMPLELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/SlipLaunchController.h b/lib/systems/include/SlipLaunchController.h new file mode 100644 index 000000000..f7bda5521 --- /dev/null +++ b/lib/systems/include/SlipLaunchController.h @@ -0,0 +1,34 @@ +#ifndef __SLIPLAUNCHCONTROLLER_H__ +#define __SLIPLAUNCHCONTROLLER_H__ +#include "SimpleLaunchController.h" + +namespace SlipLaunchControllerParams +{ + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + const float DEFAULT_SLIP_RATIO = 0.2f; +}; +class TorqueControllerSlipLaunch : 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(float slip_ratio, int16_t initial_speed_target) + : BaseLaunchController(initial_speed_target), + slip_ratio_(slip_ratio) {} + + TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} + + LaunchStates_e get_launch_state() override { return launch_state_; } + + void calc_launch_algo(const vectornav &vn_data) override; +}; +#endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMuxV2.h b/lib/systems/include/TorqueControllerMuxV2.h index b6863c667..2e13261b6 100644 --- a/lib/systems/include/TorqueControllerMuxV2.h +++ b/lib/systems/include/TorqueControllerMuxV2.h @@ -19,10 +19,11 @@ // TODOs // - [-] make the controllers inherit from the base controller class // - [x] port TorqueControllerSimple -// - [ ] port TorqueControllerLoadCellVectoring -// - [ ] port BaseLaunchController -// - [ ] port TorqueControllerSimpleLaunch -// - [ ] port TorqueControllerLookupLaunch +// - [x] port TorqueControllerLoadCellVectoring +// - [x] port BaseLaunchController +// - [x] port TorqueControllerSimpleLaunch +// - [x] port slip launch +// - [x] port TorqueControllerLookupLaunch // - [ ] port CASE // - [ ] integrate into state machine // - [ ] set the car_state from all sources @@ -43,11 +44,7 @@ namespace TC_MUX_DEFAULT_PARAMS // namespace TC_MUX // { -template -struct check_tc_mux -{ - -}; + template class TorqueControllerMuxv2 { diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 714faa562..3909b498b 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -32,12 +32,6 @@ const float AMK_MAX_RPM = 20000; const float AMK_MAX_TORQUE = 21.42; const float MAX_REGEN_TORQUE = 10.0; -/* LAUNCH CONSTANTS */ - -const float DEFAULT_LAUNCH_RATE = 11.76; -const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; - -const float DEFAULT_SLIP_RATIO = 0.2f; const float const_accel_time = 100; // time to use launch speed target in ms const float launch_ready_accel_threshold = .1; @@ -73,14 +67,6 @@ enum TorqueController_e TC_NUM_CONTROLLERS = 7, }; -enum class LaunchStates_e -{ - NO_LAUNCH_MODE, - LAUNCH_NOT_READY, - LAUNCH_READY, - LAUNCHING -}; - /* TORQUE CONTROLLERS */ /* diff --git a/lib/systems/src/BaseLaunchController.cpp b/lib/systems/src/BaseLaunchController.cpp new file mode 100644 index 000000000..113980538 --- /dev/null +++ b/lib/systems/src/BaseLaunchController.cpp @@ -0,0 +1,114 @@ +#include "BaseLaunchController.h" + +void BaseLaunchController::tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + const float wheel_rpms[], + const vectornav &vn_data) +{ + + if (tick.triggers.trigger100) + { + + current_millis_ = tick.millis; + + int16_t brake_torque_req = pedalsData.regenPercent * MAX_REGEN_TORQUE; + + float max_speed = 0; + for (int i = 0; i < 4; i++) + { + max_speed = std::max(max_speed, abs(wheel_rpms[i])); + } + + writeout_.ready = true; + + switch (launch_state_) + { + case LaunchStates_e::LAUNCH_NOT_READY: + // set torques and speed to 0 + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + 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; + + // init launch vars + launch_speed_target_ = 0; + time_of_launch_ = tick.millis; + // check speed is 0 and pedals not pressed + if ((pedalsData.accelPercent < launch_ready_accel_threshold) && (pedalsData.brakePercent < launch_ready_brake_threshold) && (max_speed < launch_ready_speed_threshold)) + { + launch_state_ = LaunchStates_e::LAUNCH_READY; + } + + break; + case LaunchStates_e::LAUNCH_READY: + // set torques and speed to 0 + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + 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; + + // init launch vars + launch_speed_target_ = 0; + time_of_launch_ = current_millis_; + + // check speed is 0 and brake not pressed + if ((pedalsData.brakePercent >= launch_ready_brake_threshold) || (max_speed >= launch_ready_speed_threshold)) + { + launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; + } + else if (pedalsData.accelPercent >= launch_go_accel_threshold) + { + + initial_ecef_x_ = vn_data.ecef_coords[0]; + initial_ecef_y_ = vn_data.ecef_coords[1]; + initial_ecef_z_ = vn_data.ecef_coords[2]; + + launch_state_ = LaunchStates_e::LAUNCHING; + } + + // check accel above launch threshold and launch + break; + case LaunchStates_e::LAUNCHING: + { // use brackets to ignore 'cross initialization' of secs_since_launch + // check accel below launch threshold and brake above + if ((pedalsData.accelPercent <= launch_stop_accel_threshold) || (pedalsData.brakePercent >= launch_ready_brake_threshold)) + { + launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; + } + + calc_launch_algo(vn_data); + + writeout_.command.speeds_rpm[FL] = launch_speed_target_; + writeout_.command.speeds_rpm[FR] = launch_speed_target_; + writeout_.command.speeds_rpm[RL] = launch_speed_target_; + writeout_.command.speeds_rpm[RR] = launch_speed_target_; + + writeout_.command.torqueSetpoints[FL] = AMK_MAX_TORQUE; + writeout_.command.torqueSetpoints[FR] = AMK_MAX_TORQUE; + writeout_.command.torqueSetpoints[RL] = AMK_MAX_TORQUE; + writeout_.command.torqueSetpoints[RR] = AMK_MAX_TORQUE; + } + break; + default: + break; + } + } +} + + +TorqueControllerOutput_s BaseLaunchController::evaluate(const car_state &state) +{ + tick(state.systick, state.pedals_data, state.drivetrain_data.measuredSpeeds, state.vn_data); + return writeout_; +} \ No newline at end of file diff --git a/lib/systems/src/LaunchControllerAlgos.cpp b/lib/systems/src/LaunchControllerAlgos.cpp new file mode 100644 index 000000000..3aadd062b --- /dev/null +++ b/lib/systems/src/LaunchControllerAlgos.cpp @@ -0,0 +1,66 @@ +#include "LookupLaunchController.h" +#include "SlipLaunchController.h" +#include "SimpleLaunchController.h" + +void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav &vn_data) +{ + /* + Stolen launch algo from HT07. This ramps up the speed target over time. + launch rate target is m/s^2 and is the target acceleration rate + secs_since_launch takes the milliseconds since launch started and converts to sec + This is then converted to RPM for a speed target + There is an initial speed target that is your iitial instant acceleration on the wheels + */ + float secs_since_launch = (float)(current_millis_ - time_of_launch_) / 1000.0; + launch_speed_target_ = (int16_t)((float)secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); + launch_speed_target_ += init_speed_target_; + launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); +} + +void TorqueControllerLookupLaunch::calc_launch_algo(const vectornav &vn_data) +{ + + launch_speed_target_ = std::max((float)LookupLaunchControllerParams::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 TorqueControllerSlipLaunch::calc_launch_algo(const vectornav &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)SlipLaunchControllerParams::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); +} \ No newline at end of file diff --git a/lib/systems/src/LoadCellVectoringController.cpp b/lib/systems/src/LoadCellVectoringController.cpp index a8e99e824..ebbec934d 100644 --- a/lib/systems/src/LoadCellVectoringController.cpp +++ b/lib/systems/src/LoadCellVectoringController.cpp @@ -4,7 +4,6 @@ void TorqueControllerLoadCellVectoring::tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, - float torqueLimit, const LoadCellInterfaceOutput_s &loadCellData ) { @@ -96,5 +95,6 @@ void TorqueControllerLoadCellVectoring::tick( TorqueControllerOutput_s TorqueControllerLoadCellVectoring::evaluate(const car_state &state) { - tick(state.systick, state.) + tick(state.systick, state.pedals_data, state.loadcell_data); + return writeout_; } \ No newline at end of file From 87009fbe7fbf502302a3dbe697dfc1ba183c0b61 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 11 May 2024 04:00:00 -0400 Subject: [PATCH 04/31] working through integration --- lib/interfaces/include/DashboardInterface.h | 10 +- lib/interfaces/include/LoadCellInterface.h | 7 +- lib/interfaces/include/MCUInterface.h | 1 + lib/interfaces/include/TelemetryInterface.h | 3 +- lib/interfaces/src/DashboardInterface.cpp | 79 +++-- lib/interfaces/src/TelemetryInterface.cpp | 106 +++--- lib/mock_interfaces/AnalogSensorsInterface.h | 19 +- lib/mock_interfaces/DashboardInterface.h | 10 +- lib/mock_interfaces/HytechCANInterface.h | 9 + lib/shared_data/include/PhysicalParameters.h | 7 + lib/shared_data/include/SharedDataTypes.h | 83 ++++- lib/state_machine/include/MCUStateMachine.h | 16 +- lib/state_machine/include/MCUStateMachine.tpp | 28 +- lib/systems/include/BaseController.h | 6 + lib/systems/include/BaseLaunchController.h | 20 +- lib/systems/include/CASEControllerWrapper.h | 19 +- lib/systems/include/CASESystem.h | 4 + lib/systems/include/CASESystem.tpp | 3 +- .../include/LoadCellVectoringController.h | 4 +- lib/systems/include/LookupLaunchController.h | 7 +- lib/systems/include/SimpleController.h | 13 +- lib/systems/include/SimpleLaunchController.h | 10 +- lib/systems/include/SlipLaunchController.h | 6 +- lib/systems/include/SteeringSystem.h | 15 +- lib/systems/include/TorqueControllerMux.h | 232 ++++++------ ...ollerMuxV2.tpp => TorqueControllerMux.tpp} | 22 +- lib/systems/include/TorqueControllerMuxV2.h | 114 ------ lib/systems/include/TorqueControllers.h | 318 +---------------- lib/systems/src/BaseLaunchController.cpp | 20 +- lib/systems/src/LaunchControllerAlgos.cpp | 3 +- .../src/LoadCellVectoringController.cpp | 14 +- lib/systems/src/SimpleController.cpp | 20 +- lib/systems/src/TorqueControllerMux.cpp | 227 ------------ lib/systems/src/TorqueControllers.cpp | 332 ------------------ platformio.ini | 2 +- src/main.cpp | 22 +- test/test_systems/fake_controller_type.h | 13 + test/test_systems/state_machine_test.h | 135 +++---- test/test_systems/tc_mux_v2_testing.h | 78 +++- test/test_systems/test_CASE.h | 10 +- 40 files changed, 621 insertions(+), 1426 deletions(-) rename lib/systems/include/{TorqueControllerMuxV2.tpp => TorqueControllerMux.tpp} (85%) delete mode 100644 lib/systems/include/TorqueControllerMuxV2.h delete mode 100644 lib/systems/src/TorqueControllerMux.cpp delete mode 100644 lib/systems/src/TorqueControllers.cpp create mode 100644 test/test_systems/fake_controller_type.h diff --git a/lib/interfaces/include/DashboardInterface.h b/lib/interfaces/include/DashboardInterface.h index f79a318aa..556cae215 100644 --- a/lib/interfaces/include/DashboardInterface.h +++ b/lib/interfaces/include/DashboardInterface.h @@ -7,7 +7,6 @@ #include "MCUInterface.h" #include "InverterInterface.h" #include "SharedDataTypes.h" -#include "TorqueControllers.h" @@ -51,6 +50,7 @@ struct DashButtons_s bool right_shifter; }; + /* Struct holding all data for the DashboardInterface (inputs and outputs) */ struct DashComponentInterface_s { @@ -58,6 +58,7 @@ struct DashComponentInterface_s // enum for dial position read by controller mux ControllerMode_e dial_mode; ControllerMode_e cur_dial_mode; + TorqueLimit_e torque_limit_mode; // Buttons struct for better naming DashButtons_s button; bool ssok; // safety system OK (IMD?) RENAME @@ -85,7 +86,8 @@ class DashboardInterface CANBufferType *msg_queue_; /* The instantiated data struct used to access data by member functions */ DashComponentInterface_s _data; - + bool prev_button_pressed_state_; + update_torque_mode_(bool button_pressed); public: /*! Constructor for new DashboardInterface, All that it is inited with @@ -95,6 +97,8 @@ class DashboardInterface */ DashboardInterface(CANBufferType *msg_output_queue) { + torque_limit_mode_ = TorqueLimit_e::TCMUX_FULL_TORQUE; + prev_button_pressed_state_ = false; msg_queue_ = msg_output_queue; }; @@ -126,7 +130,7 @@ class DashboardInterface @return returns a ControllerMode_e enum with the current dial position */ ControllerMode_e getDialMode(); - + TorqueLimit_e getTorqueLimitMode(); /* gets whether the safety system is ok (wtf is a safety system - rename this)*/ bool safetySystemOK(); diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index adf85befb..7d5dc4d0c 100644 --- a/lib/interfaces/include/LoadCellInterface.h +++ b/lib/interfaces/include/LoadCellInterface.h @@ -4,6 +4,7 @@ #include "Utility.h" #include "SysClock.h" #include "AnalogSensorsInterface.h" +#include "SharedDataTypes.h" /* Structs */ @@ -15,12 +16,6 @@ struct LoadCellInterfaceTick_s const AnalogConversion_s &RRConversion; }; -struct LoadCellInterfaceOutput_s -{ - veh_vec loadCellForcesFiltered; - veh_vec loadCellConversions; - bool FIRSaturated; -}; class LoadCellInterface { diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index 521f10301..2c7f1d580 100644 --- a/lib/interfaces/include/MCUInterface.h +++ b/lib/interfaces/include/MCUInterface.h @@ -7,6 +7,7 @@ #include "hytech.h" #include "MessageQueueDefine.h" #include "PedalsSystem.h" +#include "SharedDataTypes.h" const int DEFAULT_BMS_OK_READ = 17; // SHDN_D_READ const int DEFAULT_BMS_SENSE_PIN = 16; // BMS_OK_SENSE diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index d8008e80c..cf187839f 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -147,7 +147,8 @@ class TelemetryInterface const AnalogConversion_s &brake_1, const AnalogConversion_s &brake_2, float mech_brake_active_percent, - const PIDTVTorqueControllerData &data); + const PIDTVTorqueControllerData &data, + const TorqueControllerMuxError& current_mux_status); }; #endif /* TELEMETRYINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index c0ec256c6..a5ca43b4a 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -7,7 +7,7 @@ void DashboardInterface::read(const CAN_message_t &can_msg) Unpack_DASHBOARD_STATE_hytech(&dash_state, can_msg.buf, can_msg.len); _data.dial_mode = static_cast(dash_state.dial_state); - + _data.ssok = dash_state.ssok_above_threshold; _data.shutdown = dash_state.shutdown_h_above_threshold; @@ -23,10 +23,23 @@ void DashboardInterface::read(const CAN_message_t &can_msg) _data.buzzer_state = dash_state.drive_buzzer; + update_torque_mode_(_data.button.torque_mode); +} + +// TODO this should be part of the CAN message not in this interface. +void DashboardInterface::update_torque_mode_(bool button_pressed) +{ + // detect high-to-low transition + + if (prev_button_pressed_state_ == true && button_pressed == false) + { + _data.torque_limit_mode_ = static_cast((static_cast(_data.torque_limit_mode_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); + } + prev_button_pressed_state_ = button_pressed; } CAN_message_t DashboardInterface::write() -{ +{ DASHBOARD_MCU_STATE_t dash_mcu_state; dash_mcu_state.drive_buzzer = _data.buzzer_cmd; @@ -45,12 +58,12 @@ CAN_message_t DashboardInterface::write() dash_mcu_state.bots_led = _data.LED[static_cast(DashLED_e::BOTS_LED)]; dash_mcu_state.imd_led = _data.LED[static_cast(DashLED_e::IMD_LED)]; dash_mcu_state.ams_led = _data.LED[static_cast(DashLED_e::AMS_LED)]; - + dash_mcu_state.glv_led = _data.LED[static_cast(DashLED_e::GLV_LED)]; dash_mcu_state.pack_charge_led = _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)]; - + CAN_message_t can_msg; - auto id = Pack_DASHBOARD_MCU_STATE_hytech(&dash_mcu_state, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + auto id = Pack_DASHBOARD_MCU_STATE_hytech(&dash_mcu_state, can_msg.buf, &can_msg.len, (uint8_t *)&can_msg.flags.extended); can_msg.id = id; // this circular buffer implementation requires that you push your data in a array buffer // all this does is put the msg into a uint8_t buffer and pushes it onto the queue @@ -59,20 +72,19 @@ CAN_message_t DashboardInterface::write() msg_queue_->push_back(buf, sizeof(CAN_message_t)); return can_msg; - } -//figure out how to set enumed led colors or send (0,255 value) +// figure out how to set enumed led colors or send (0,255 value) void DashboardInterface::setLED(DashLED_e led, LEDColors_e color) { _data.LED[static_cast(led)] = static_cast(color); } -void DashboardInterface::tick10(MCUInterface* mcu, - int car_state, - bool buzzer, - bool drivetrain_error, +void DashboardInterface::tick10(MCUInterface *mcu, + int car_state, + bool buzzer, + bool drivetrain_error, TorqueLimit_e torque, float min_cell_voltage, AnalogConversion_s glv_voltage, @@ -80,8 +92,8 @@ void DashboardInterface::tick10(MCUInterface* mcu, ControllerMode_e dial_mode) { + // TODO unfuck this _data.cur_dial_mode = dial_mode; - soundBuzzer(buzzer); setLED(DashLED_e::AMS_LED, mcu->bms_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); @@ -91,7 +103,8 @@ void DashboardInterface::tick10(MCUInterface* mcu, setLED(DashLED_e::MC_ERROR_LED, !drivetrain_error ? LEDColors_e::ON : LEDColors_e::RED); setLED(DashLED_e::COCKPIT_BRB_LED, mcu->brb_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); - switch(launch_state){ + switch (launch_state) + { case 1: setLED(DashLED_e::LAUNCH_CONTROL_LED, LEDColors_e::RED); break; @@ -106,7 +119,8 @@ void DashboardInterface::tick10(MCUInterface* mcu, break; } - switch(torque){ + switch (torque) + { case TorqueLimit_e::TCMUX_LOW_TORQUE: setLED(DashLED_e::MODE_LED, LEDColors_e::OFF); break; @@ -121,27 +135,26 @@ void DashboardInterface::tick10(MCUInterface* mcu, break; } - uint16_t scaled_cell_voltage = (uint16_t)map((uint32_t)(min_cell_voltage*1000), 3300, 4200, 0, 255);// scale voltage - _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)] = std::max(0, std::min((int)scaled_cell_voltage, 255));// clamp voltage + uint16_t scaled_cell_voltage = (uint16_t)map((uint32_t)(min_cell_voltage * 1000), 3300, 4200, 0, 255); // scale voltage + _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)] = std::max(0, std::min((int)scaled_cell_voltage, 255)); // clamp voltage // _data.LED[DashLED_e::GLV_LED] = (uint8_t)map(glv_voltage.raw) write(); } -ControllerMode_e DashboardInterface::getDialMode() {return _data.dial_mode;} - -bool DashboardInterface::startButtonPressed() {return _data.button.start;} -bool DashboardInterface::specialButtonPressed() {return _data.button.mark;} -bool DashboardInterface::torqueModeButtonPressed() {return _data.button.torque_mode;} -bool DashboardInterface::inverterResetButtonPressed() {return _data.button.mc_cycle;} -bool DashboardInterface::launchControlButtonPressed() {return _data.button.launch_ctrl;} -bool DashboardInterface::nightModeButtonPressed() {return _data.button.led_dimmer;} -bool DashboardInterface::leftShifterButtonPressed() {return _data.button.left_shifter;} -bool DashboardInterface::rightShifterButtonPressed() {return _data.button.right_shifter;} - -bool DashboardInterface::safetySystemOK() {return _data.ssok;} -bool DashboardInterface::shutdownHAboveThreshold() {return _data.shutdown;} - -void DashboardInterface::soundBuzzer(bool state) {_data.buzzer_cmd = state;} -bool DashboardInterface::checkBuzzer() {return _data.buzzer_state;} - +ControllerMode_e DashboardInterface::getDialMode() { return _data.dial_mode; } +TorqueLimit_e DashboardInterface::getTorqueLimitMode() {return _data.torque_limit_mode; } +bool DashboardInterface::startButtonPressed() { return _data.button.start; } +bool DashboardInterface::specialButtonPressed() { return _data.button.mark; } +bool DashboardInterface::torqueModeButtonPressed() { return _data.button.torque_mode; } +bool DashboardInterface::inverterResetButtonPressed() { return _data.button.mc_cycle; } +bool DashboardInterface::launchControlButtonPressed() { return _data.button.launch_ctrl; } +bool DashboardInterface::nightModeButtonPressed() { return _data.button.led_dimmer; } +bool DashboardInterface::leftShifterButtonPressed() { return _data.button.left_shifter; } +bool DashboardInterface::rightShifterButtonPressed() { return _data.button.right_shifter; } + +bool DashboardInterface::safetySystemOK() { return _data.ssok; } +bool DashboardInterface::shutdownHAboveThreshold() { return _data.shutdown; } + +void DashboardInterface::soundBuzzer(bool state) { _data.buzzer_cmd = state; } +bool DashboardInterface::checkBuzzer() { return _data.buzzer_state; } diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 963a83740..09e97089a 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -4,20 +4,22 @@ // MCP3208 returns structure void TelemetryInterface::update_pedal_readings_CAN_msg(float accel_percent, float brake_percent, - float mech_brake_percent) { + float mech_brake_percent) +{ MCU_PEDAL_READINGS_t pedal_readings; - pedal_readings.accel_percent_float_ro = HYTECH_accel_percent_float_ro_toS(accel_percent*100); - pedal_readings.brake_percent_float_ro = HYTECH_brake_percent_float_ro_toS(brake_percent*100); - pedal_readings.mechanical_brake_percent_float_ro = HYTECH_mechanical_brake_percent_float_ro_toS(mech_brake_percent*100); + pedal_readings.accel_percent_float_ro = HYTECH_accel_percent_float_ro_toS(accel_percent * 100); + pedal_readings.brake_percent_float_ro = HYTECH_brake_percent_float_ro_toS(brake_percent * 100); + pedal_readings.mechanical_brake_percent_float_ro = HYTECH_mechanical_brake_percent_float_ro_toS(mech_brake_percent * 100); enqueue_new_CAN(&pedal_readings, &Pack_MCU_PEDAL_READINGS_hytech); } void TelemetryInterface::update_pedal_readings_raw_CAN_msg(const AnalogConversion_s &accel_1, const AnalogConversion_s &accel_2, const AnalogConversion_s &brake_1, - const AnalogConversion_s &brake_2) { + const AnalogConversion_s &brake_2) +{ MCU_PEDAL_RAW_t pedal_read; pedal_read.accel_1_raw = accel_1.raw; @@ -26,13 +28,13 @@ void TelemetryInterface::update_pedal_readings_raw_CAN_msg(const AnalogConversio pedal_read.brake_2_raw = brake_2.raw; enqueue_new_CAN(&pedal_read, &Pack_MCU_PEDAL_RAW_hytech); - } // MCP3204 returns structure void TelemetryInterface::update_suspension_CAN_msg(const AnalogConversion_s &lc_fl, const AnalogConversion_s &lc_fr, const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr) { + const AnalogConversion_s &pots_fr) +{ MCU_SUSPENSION_t sus; sus.load_cell_fl = lc_fl.raw; sus.load_cell_fr = lc_fr.raw; @@ -46,7 +48,8 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon const AnalogConversion_s &steer2, const AnalogConversion_s ¤t, const AnalogConversion_s &reference, - const AnalogConversion_s &glv) { + const AnalogConversion_s &glv) +{ // do sth with mcu_analog_readings_ mcu_analog_readings_.set_steering_1(steer1.raw); mcu_analog_readings_.set_steering_2(steer2.raw); @@ -58,8 +61,9 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon enqueue_CAN(mcu_analog_readings_, ID_MCU_ANALOG_READINGS); } void TelemetryInterface::update_front_thermistors_CAN_msg(const AnalogConversion_s &therm_fl, - const AnalogConversion_s &therm_fr) { - + const AnalogConversion_s &therm_fr) +{ + FRONT_THERMISTORS_t front_thermistors_; front_thermistors_.thermistor_motor_fl = therm_fl.raw; front_thermistors_.thermistor_motor_fr = therm_fr.raw; @@ -67,22 +71,22 @@ void TelemetryInterface::update_front_thermistors_CAN_msg(const AnalogConversion enqueue_new_CAN(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); } - -void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) +void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, InvInt_t *rr) { DRIVETRAIN_RPMS_TELEM_t rpms; rpms.fl_motor_rpm = fl->get_speed(); rpms.fr_motor_rpm = fr->get_speed(); rpms.rl_motor_rpm = rl->get_speed(); rpms.rr_motor_rpm = rr->get_speed(); - + enqueue_new_CAN(&rpms, &Pack_DRIVETRAIN_RPMS_TELEM_hytech); } -void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) +void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, InvInt_t *rr) { - if (1) { // We should only write error status if some error has occurred, but for now, this is just an if(1) + if (1) + { // We should only write error status if some error has occurred, but for now, this is just an if(1) DRIVETRAIN_ERR_STATUS_TELEM_t errors; errors.mc1_diagnostic_number = fl->get_error_status(); errors.mc2_diagnostic_number = fr->get_error_status(); @@ -90,18 +94,17 @@ void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t* fl, InvI errors.mc4_diagnostic_number = rr->get_error_status(); enqueue_new_CAN(&errors, &Pack_DRIVETRAIN_ERR_STATUS_TELEM_hytech); } - } void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( - InvInt_t* fl, - InvInt_t* fr, - InvInt_t* rl, - InvInt_t* rr, - bool accel_implaus, - bool brake_implaus, - float accel_per, - float brake_per) + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr, + bool accel_implaus, + bool brake_implaus, + float accel_per, + float brake_per) { DRIVETRAIN_STATUS_TELEM_t status; @@ -150,7 +153,7 @@ void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( MC_temps mc2_temps = fr->get_temps_msg(); MC_temps mc3_temps = rl->get_temps_msg(); MC_temps mc4_temps = rr->get_temps_msg(); - + MC_setpoints_command mc1_cmd = fl->get_cmd_msg(); MC_setpoints_command mc2_cmd = fr->get_cmd_msg(); MC_setpoints_command mc3_cmd = rl->get_cmd_msg(); @@ -168,10 +171,10 @@ void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( } void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg( - InvInt_t* fl, - InvInt_t* fr, - InvInt_t* rl, - InvInt_t* rr) + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr) { // TODO: change this to use actual torque values from inverter // Torque current just temporary for gearbox seal validation @@ -195,9 +198,10 @@ void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s } /* Send CAN messages */ -template -void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { - +template +void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) +{ + CAN_message_t msg; msg_class.write(msg.buf); msg.id = id; @@ -206,20 +210,20 @@ void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { memmove(buf, &msg, sizeof(CAN_message_t)); msg_queue_->push_back(buf, sizeof(CAN_message_t)); - } /* Send inverter CAN messages with new CAN library */ -template -void TelemetryInterface::enqueue_new_CAN(U* structure, uint32_t (* pack_function)(U*, uint8_t*, uint8_t*, uint8_t*)) { +template +void TelemetryInterface::enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *)) +{ CAN_message_t can_msg; - can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t *)&can_msg.flags.extended); uint8_t buf[sizeof(CAN_message_t)] = {}; memmove(buf, &can_msg, sizeof(CAN_message_t)); msg_queue_->push_back(buf, sizeof(CAN_message_t)); } -void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerData& data) +void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data) { CONTROLLER_PID_TV_DATA_t msg; msg.controller_input_ro = HYTECH_controller_input_ro_toS(data.controller_input); @@ -231,23 +235,20 @@ void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerDa delta_msg.pid_tv_fr_delta_ro = HYTECH_pid_tv_fr_delta_ro_toS(data.fr_torque_delta); delta_msg.pid_tv_rl_delta_ro = HYTECH_pid_tv_rl_delta_ro_toS(data.rl_torque_delta); delta_msg.pid_tv_rr_delta_ro = HYTECH_pid_tv_rr_delta_ro_toS(data.rr_torque_delta); - - enqueue_new_CAN(&delta_msg, &Pack_CONTROLLER_PID_TV_DELTA_DATA_hytech); - + enqueue_new_CAN(&delta_msg, &Pack_CONTROLLER_PID_TV_DELTA_DATA_hytech); } - /* Tick SysClock */ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, const AnalogConversionPacket_s<2> &mcu_adc, const SteeringEncoderConversion_s &encoder, - InvInt_t* fl, - InvInt_t* fr, - InvInt_t* rl, - InvInt_t* rr, + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr, bool accel_implaus, bool brake_implaus, float accel_per, @@ -257,17 +258,23 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversion_s &brake_1, const AnalogConversion_s &brake_2, float mech_brake_active_percent, - const PIDTVTorqueControllerData& data) { + const PIDTVTorqueControllerData &data, + const TorqueControllerMuxError ¤t_mux_status) +{ + + MCU_ERROR_STATES_t error_states; + error_states.torque_controller_mux_status = static_cast(current_mux_status); + enqueue_new_CAN(&error_states, &Pack_MCU_ERROR_STATES_hytech); // Pedals update_pedal_readings_CAN_msg(accel_per, brake_per, mech_brake_active_percent); - + update_pedal_readings_raw_CAN_msg(accel_1, accel_2, brake_1, - brake_2); + brake_2); // Analog readings update_analog_readings_CAN_msg(encoder, adc1.conversions[channels_.analog_steering_channel], @@ -290,6 +297,5 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, enqeue_controller_CAN_msg(data); update_front_thermistors_CAN_msg(mcu_adc.conversions[channels_.therm_fl_channel], - mcu_adc.conversions[channels_.therm_fr_channel]); - + mcu_adc.conversions[channels_.therm_fr_channel]); } \ No newline at end of file diff --git a/lib/mock_interfaces/AnalogSensorsInterface.h b/lib/mock_interfaces/AnalogSensorsInterface.h index 2fb8f1fdf..98d6d699e 100644 --- a/lib/mock_interfaces/AnalogSensorsInterface.h +++ b/lib/mock_interfaces/AnalogSensorsInterface.h @@ -4,24 +4,7 @@ #include #include -enum class AnalogSensorStatus_e -{ - ANALOG_SENSOR_GOOD = 0, - ANALOG_SENSOR_CLAMPED = 1, -}; - -struct AnalogConversion_s -{ - int raw; - float conversion; - AnalogSensorStatus_e status; -}; - -template -struct AnalogConversionPacket_s -{ - AnalogConversion_s conversions[N]; -}; +#include "SharedDataTypes.h" class AnalogChannel { diff --git a/lib/mock_interfaces/DashboardInterface.h b/lib/mock_interfaces/DashboardInterface.h index 46be692fa..57007a569 100644 --- a/lib/mock_interfaces/DashboardInterface.h +++ b/lib/mock_interfaces/DashboardInterface.h @@ -2,14 +2,6 @@ #define DASHBOARDINTERFACE #include "SharedDataTypes.h" -/* - Enum for the car's torque limits - MOVE ME! - ideally into a TorqueControllerDefs.h file - to prevent circular dependencies -*/ - - - /* Enum for defined LED colors. ON will be LED's default color on dashboard*/ enum class LEDColors_e @@ -43,6 +35,8 @@ class DashboardInterface public: bool buzzer = false; bool start_button_status_; + ControllerMode_e getDialMode() {return {};} + TorqueLimit_e getTorqueLimitMode() {return {};} bool startButtonPressed() { return start_button_status_; }; bool checkBuzzer(){ return buzzer; }; }; diff --git a/lib/mock_interfaces/HytechCANInterface.h b/lib/mock_interfaces/HytechCANInterface.h index e69de29bb..2628c9f49 100644 --- a/lib/mock_interfaces/HytechCANInterface.h +++ b/lib/mock_interfaces/HytechCANInterface.h @@ -0,0 +1,9 @@ +#ifndef __HYTECHCANINTERFACE_H__ +#define __HYTECHCANINTERFACE_H__ +#include "HT08_CASE_types.h" + +template +void enqueue_matlab_msg(bufferType *msg_queue, const CAN_MESSAGE_BUS & structure) +{ +} +#endif // __HYTECHCANINTERFACE_H__ \ No newline at end of file diff --git a/lib/shared_data/include/PhysicalParameters.h b/lib/shared_data/include/PhysicalParameters.h index 7f971575a..916548537 100644 --- a/lib/shared_data/include/PhysicalParameters.h +++ b/lib/shared_data/include/PhysicalParameters.h @@ -1,6 +1,13 @@ #ifndef __PHYSICALPARAMETERS_H__ #define __PHYSICALPARAMETERS_H__ +namespace PhysicalParameters +{ + const float AMK_MAX_RPM = 20000; + const float AMK_MAX_TORQUE = 21.42; + const float MAX_REGEN_TORQUE = 10.0; + +} const float GEARBOX_RATIO = 11.86; const float WHEEL_DIAMETER = 0.4064; // meters const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 9d1468e94..33eea4298 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -2,6 +2,25 @@ #define TORQUECONTROLLERSDATA #include #include "Utility.h" +#include "SysClock.h" +enum class AnalogSensorStatus_e +{ + ANALOG_SENSOR_GOOD = 0, + ANALOG_SENSOR_CLAMPED = 1, +}; + +struct AnalogConversion_s +{ + int raw; + float conversion; + AnalogSensorStatus_e status; +}; + +template +struct AnalogConversionPacket_s +{ + AnalogConversion_s conversions[N]; +}; struct PIDTVTorqueControllerData { @@ -14,9 +33,9 @@ struct PIDTVTorqueControllerData }; enum class TorqueLimit_e { - TCMUX_LOW_TORQUE = 0, + TCMUX_FULL_TORQUE = 0, TCMUX_MID_TORQUE = 1, - TCMUX_FULL_TORQUE = 2, + TCMUX_LOW_TORQUE = 2, TCMUX_NUM_TORQUE_LIMITS = 3, }; @@ -62,10 +81,6 @@ struct DrivetrainCommand_s float torqueSetpoints[NUM_MOTORS]; // FIXME: misnomer. This represents the magnitude of the torque the inverter can command to reach the commanded speed setpoint }; -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}}; - struct TorqueControllerOutput_s { DrivetrainCommand_s command; @@ -105,6 +120,46 @@ struct TorqueControllerMuxStatus bool output_is_bypassing_limits; }; +struct LoadCellInterfaceOutput_s +{ + veh_vec loadCellForcesFiltered; + veh_vec loadCellConversions; + bool FIRSaturated; +}; + +// Enums +enum class SteeringSystemStatus_e +{ + STEERING_SYSTEM_NOMINAL = 0, + STEERING_SYSTEM_MARGINAL = 1, + STEERING_SYSTEM_DEGRADED = 2, + STEERING_SYSTEM_ERROR = 3, +}; + +struct SteeringSystemData_s +{ + float angle; + SteeringSystemStatus_e status; +}; + +// struct TriggerBits_s +// { +// bool trigger1000 : 1; +// bool trigger500 : 1; +// bool trigger100 : 1; +// bool trigger50 : 1; +// bool trigger10 : 1; +// bool trigger5 : 1; +// bool trigger1 : 1; +// }; + +// struct SysTick_s +// { +// unsigned long millis; +// unsigned long micros; +// TriggerBits_s triggers; +// }; + struct car_state { // data @@ -114,6 +169,22 @@ struct car_state LoadCellInterfaceOutput_s loadcell_data; PedalsSystemData_s pedals_data; vectornav vn_data; + car_state() = delete; + car_state(const SysTick_s &_systick, + const SteeringSystemData_s &_steering_data, + const DrivetrainDynamicReport_s &_drivetrain_data, + const LoadCellInterfaceOutput_s &_loadcell_data, + const PedalsSystemData_s &_pedals_data, + const vectornav &_vn_data) + : systick(_systick), + steering_data(_steering_data), + drivetrain_data(_drivetrain_data), + loadcell_data(_loadcell_data), + pedals_data(_pedals_data), + vn_data(_vn_data) + { + // constructor body (if needed) + } }; #endif \ No newline at end of file diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index 7668ad8cf..e526fe39f 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -24,7 +24,7 @@ enum class CAR_STATE READY_TO_DRIVE = 5 }; -template +template class MCUStateMachine { public: @@ -32,7 +32,7 @@ class MCUStateMachine DrivetrainSysType *drivetrain, DashboardInterface *dashboard, PedalsSystem *pedals, - TorqueControllerMux *mux, + TCMuxType *mux, SafetySystem *safety_system) { current_state_ = CAR_STATE::STARTUP; @@ -44,10 +44,12 @@ class MCUStateMachine safety_system_ = safety_system; } - /// @brief our components can use this time to tell when to do things. We can set this ourselves for testing purposes instead of using metro timers - /// @param current_millis the current millis() call - // void tick_state_machine(const SysTick_s &tick); - void tick_state_machine(unsigned long cm); + // TODO update this to just use the car_state struct instead of having to pass in cm + /// @brief function to tick the state machine. + /// @param cm current millis from systick + /// @param current_car_state current state of the car (not to be confused with the CAR_STATE enum which is the state machine state) + void tick_state_machine(unsigned long cm, const car_state ¤t_car_state); + CAR_STATE get_state() { return current_state_; } bool car_in_ready_to_drive() { return current_state_ == CAR_STATE::READY_TO_DRIVE; }; @@ -72,7 +74,7 @@ class MCUStateMachine DashboardInterface *dashboard_; // IMDInterface *imd_; SafetySystem *safety_system_; - TorqueControllerMux *controller_mux_; + TCMuxType *controller_mux_; }; #include "MCUStateMachine.tpp" #endif /* MCUSTATEMACHINE */ diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index 6cbdcbf42..1c36ab1b2 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -1,7 +1,7 @@ #include "MCUStateMachine.h" -template -void MCUStateMachine::tick_state_machine(unsigned long current_millis) +template +void MCUStateMachine::tick_state_machine(unsigned long current_millis, const car_state ¤t_car_state) { switch (get_state()) { @@ -39,7 +39,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren // Serial.print(" "); // Serial.print(data.brakeAndAccelPressedImplausibility); // Serial.print(" "); - + // Serial.println("accel, brake:"); // Serial.print(data.accelPercent); // Serial.print(" "); @@ -48,7 +48,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren // Serial.print(data.implausibilityExceededMaxDuration); // Serial.println(); - // if TS is above HV threshold, move to Tractive System Active drivetrain_->disable_no_pins(); if (drivetrain_->hv_over_threshold_on_drivetrain()) @@ -128,7 +127,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::READY_TO_DRIVE: { auto data = pedals_->getPedalsSystemData(); - // auto test = controller_mux_->getDrivetrainCommand(); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { @@ -146,12 +144,12 @@ void MCUStateMachine::tick_state_machine(unsigned long curren if (safety_system_->get_software_is_ok() && !data.implausibilityExceededMaxDuration) { - drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand()); + + drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state)); } - else + else { drivetrain_->command_drivetrain_no_torque(); - } break; @@ -159,8 +157,8 @@ void MCUStateMachine::tick_state_machine(unsigned long curren } } -template -void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) +template +void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) { // hal_println("running exit logic"); @@ -173,8 +171,8 @@ void MCUStateMachine::set_state_(CAR_STATE new_state, unsigne handle_entry_logic_(new_state, curr_time); } -template -void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_time) +template +void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_time) { switch (get_state()) { @@ -189,15 +187,15 @@ void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: break; case CAR_STATE::READY_TO_DRIVE: - { + { // deactivate buzzer and reset it to turn on again later buzzer_->deactivate(); break; } } } -template -void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_time) +template +void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_time) { switch (new_state) { diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index 2f9d70c69..b6e76c8f4 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -1,7 +1,13 @@ #ifndef BASECONTROLLER #define BASECONTROLLER #include "SharedDataTypes.h" +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}}; +} class Controller { private: diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index cdd8f80e5..94ec879ed 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -2,7 +2,8 @@ #define __BASELAUNCHCONTROLLER_H__ #include "SharedDataTypes.h" #include "BaseController.h" - +#include +#include enum class LaunchStates_e { NO_LAUNCH_MODE, @@ -11,7 +12,18 @@ enum class LaunchStates_e LAUNCHING }; -class BaseLaunchController : public Controller +namespace BaseLaunchControllerParams +{ + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + const float const_accel_time = 100; // time to use launch speed target in ms + const float launch_ready_accel_threshold = .1; + const float launch_ready_brake_threshold = .2; + const float launch_ready_speed_threshold = 5.0 * METERS_PER_SECOND_TO_RPM; // rpm + const float launch_go_accel_threshold = .9; + const float launch_stop_accel_threshold = .5; +} + +class BaseLaunchController : public virtual Controller { protected: TorqueControllerOutput_s writeout_; @@ -28,7 +40,7 @@ class BaseLaunchController : public Controller BaseLaunchController(int16_t initial_speed_target) : init_speed_target_(initial_speed_target) { - writeout.command = TC_COMMAND_NO_TORQUE; + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = true; } @@ -36,7 +48,7 @@ class BaseLaunchController : public Controller const PedalsSystemData_s &pedalsData, const float wheel_rpms[], const vectornav &vn_data); - + LaunchStates_e get_launch_state() { return launch_state_; } virtual void calc_launch_algo(const vectornav &vn_data) = 0; TorqueControllerOutput_s evaluate(const car_state &state) override; }; diff --git a/lib/systems/include/CASEControllerWrapper.h b/lib/systems/include/CASEControllerWrapper.h index bd5ea681f..a77b02d56 100644 --- a/lib/systems/include/CASEControllerWrapper.h +++ b/lib/systems/include/CASEControllerWrapper.h @@ -4,16 +4,27 @@ #include "CASESystem.h" -class TorqueControllerCASEWrapper : +template +class TorqueControllerCASEWrapper : public virtual Controller { public: + TorqueControllerCASEWrapper() = delete; - TorqueControllerCASEWrapper(const CASESystem& case_instance) : case_instance_(case_instance) + TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) { } - TorqueControllerOutput_s evaluate(const car_state &state) override; + TorqueControllerOutput_s evaluate(const car_state &state) override + { + DrivetrainCommand_s curr_cmd = case_instance_->get_current_drive_command(); + TorqueControllerOutput_s out; + out.ready = true; + out.command = curr_cmd; + return out; + } + private: - const CASESystem& case_instance_; + CASESystem *case_instance_; + }; #endif // __CASECONTROLLERWRAPPER_H__ \ No newline at end of file diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 8c8ad87a5..0d2d1c150 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -70,6 +70,7 @@ template class CASESystem { public: + CASESystem() = delete; /// @brief constructor for state estimator system. /// @param can_queue the pointer to the message queue that will have the CAN messages put onto it /// @param send_period_ms the period in which messages will be put into the queue to be sent in milliseconds. @@ -134,6 +135,8 @@ class CASESystem // config_.yaw_pid_brakes_d = brake_d; // } + DrivetrainCommand_s get_current_drive_command() { return current_command_; } + float calculate_torque_request(const PedalsSystemData_s &pedals_data, 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 @@ -204,6 +207,7 @@ class CASESystem } private: + DrivetrainCommand_s current_command_; CASEConfiguration config_; message_queue *msg_queue_; HT08_CASE case_; diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index cdd85b7e5..7eb9ba3c0 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -228,6 +228,7 @@ DrivetrainCommand_s CASESystem::evaluate( command.speeds_rpm[2] = get_rpm_setpoint(res.FinalTorqueRL); command.speeds_rpm[3] = get_rpm_setpoint(res.FinalTorqueRR); + current_command_ = command; return command; } @@ -241,7 +242,7 @@ float CASESystem::calculate_torque_request(const PedalsSystemData if (accelRequest >= 0.0) { // Positive torque request - torqueRequest = accelRequest * AMK_MAX_TORQUE; + torqueRequest = accelRequest * max_regen_torque; } else { diff --git a/lib/systems/include/LoadCellVectoringController.h b/lib/systems/include/LoadCellVectoringController.h index 84ca1a45c..e371a8004 100644 --- a/lib/systems/include/LoadCellVectoringController.h +++ b/lib/systems/include/LoadCellVectoringController.h @@ -2,7 +2,7 @@ #define LOADCELLVECTORINGCONTROLLER #include "BaseController.h" -class TorqueControllerLoadCellVectoring : public Controller +class TorqueControllerLoadCellVectoring : public virtual Controller { private: float frontTorqueScale_ = 1.0; @@ -54,7 +54,7 @@ class TorqueControllerLoadCellVectoring : public Controller frontRegenTorqueScale_(2.0 - regenTorqueScale), rearRegenTorqueScale_(regenTorqueScale) { - writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = false; } TorqueControllerLoadCellVectoring() : TorqueControllerLoadCellVectoring(1.0, 1.0) {} diff --git a/lib/systems/include/LookupLaunchController.h b/lib/systems/include/LookupLaunchController.h index 89c4ddda7..6c0b80357 100644 --- a/lib/systems/include/LookupLaunchController.h +++ b/lib/systems/include/LookupLaunchController.h @@ -5,10 +5,9 @@ namespace LookupLaunchControllerParams { - const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; - + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET; }; -class TorqueControllerLookupLaunch : public BaseLaunchController +class TorqueControllerLookupLaunch : public virtual BaseLaunchController { private: bool init_position = false; @@ -26,8 +25,6 @@ class TorqueControllerLookupLaunch : public BaseLaunchController TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vectornav &vn_data) override; }; diff --git a/lib/systems/include/SimpleController.h b/lib/systems/include/SimpleController.h index b5a16d641..46f12db27 100644 --- a/lib/systems/include/SimpleController.h +++ b/lib/systems/include/SimpleController.h @@ -3,10 +3,12 @@ #include "BaseController.h" -class SimpleController : public Controller + + +class TorqueControllerSimple : public Controller { private: - TorqueControllerOutput_s &writeout_; + TorqueControllerOutput_s writeout_; float frontTorqueScale_ = 1.0; float rearTorqueScale_ = 1.0; float frontRegenTorqueScale_ = 1.0; @@ -17,14 +19,13 @@ class SimpleController : public Controller /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear - TorqueControllerSimple(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), + TorqueControllerSimple(float rearTorqueScale, float regenTorqueScale) + : frontTorqueScale_(2.0 - rearTorqueScale), rearTorqueScale_(rearTorqueScale), frontRegenTorqueScale_(2.0 - regenTorqueScale), rearRegenTorqueScale_(regenTorqueScale) { - writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = true; } diff --git a/lib/systems/include/SimpleLaunchController.h b/lib/systems/include/SimpleLaunchController.h index f3de1c177..aa5be8bbf 100644 --- a/lib/systems/include/SimpleLaunchController.h +++ b/lib/systems/include/SimpleLaunchController.h @@ -6,11 +6,12 @@ namespace SimpleLaunchControllerParams { const float DEFAULT_LAUNCH_RATE = 11.76; - const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET; }; +namespace SLParams = SimpleLaunchControllerParams; -class TorqueControllerSimpleLaunch : public BaseLaunchController +class TorqueControllerSimpleLaunch : public virtual BaseLaunchController { private: float launch_rate_target_; @@ -27,9 +28,8 @@ class TorqueControllerSimpleLaunch : public BaseLaunchController : BaseLaunchController(initial_speed_target), launch_rate_target_(launch_rate) {} - TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SimpleLaunchControllerParams::DEFAULT_LAUNCH_RATE, SimpleLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } + + TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} void calc_launch_algo(const vectornav &vn_data) override; }; diff --git a/lib/systems/include/SlipLaunchController.h b/lib/systems/include/SlipLaunchController.h index f7bda5521..0c80c6d55 100644 --- a/lib/systems/include/SlipLaunchController.h +++ b/lib/systems/include/SlipLaunchController.h @@ -4,10 +4,10 @@ namespace SlipLaunchControllerParams { - const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET; const float DEFAULT_SLIP_RATIO = 0.2f; }; -class TorqueControllerSlipLaunch : public BaseLaunchController +class TorqueControllerSlipLaunch : public virtual BaseLaunchController { private: float slip_ratio_; @@ -27,8 +27,6 @@ class TorqueControllerSlipLaunch : public BaseLaunchController TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - LaunchStates_e get_launch_state() override { return launch_state_; } - void calc_launch_algo(const vectornav &vn_data) override; }; #endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index afd664819..aa439c7bd 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -4,6 +4,7 @@ #include "SteeringEncoderInterface.h" #include "AnalogSensorsInterface.h" #include "SysClock.h" +#include "SharedDataTypes.h" // Digital Encoder = Primary Sensor // Analog Encoder = Secondary Sensor @@ -12,14 +13,7 @@ #define STEERING_DIVERGENCE_ERROR_THRESHOLD (14.0) // Steering sensors can disagree by 5 degrees before output is considered erroneous #define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge 2.5 degrees -// Enums -enum class SteeringSystemStatus_e -{ - STEERING_SYSTEM_NOMINAL = 0, - STEERING_SYSTEM_MARGINAL = 1, - STEERING_SYSTEM_DEGRADED = 2, - STEERING_SYSTEM_ERROR = 3, -}; + // Structs struct SteeringSystemTick_s @@ -28,11 +22,6 @@ struct SteeringSystemTick_s const AnalogConversion_s &secondaryConversion; }; -struct SteeringSystemData_s -{ - float angle; - SteeringSystemStatus_e status; -}; class SteeringSystem { diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 79e00e1c5..c27d2cd95 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -1,144 +1,124 @@ -#ifndef __TORQUECTRLMUX_H__ -#define __TORQUECTRLMUX_H__ +#ifndef TORQUECONTROLLERMUX +#define TORQUECONTROLLERMUX + +#include +#include "SharedDataTypes.h" +#include "BaseController.h" + +// notes: +// 21 torque limit should be first + +// tc mux needs to handle these things: +// 1 swapping between controller outputs +// 2 turning on and off running of controllers +// 3 application of safeties and limits to controller outputs +// 4 torque limit changing (torque mode) --> +// TODO the torque limit value changing should be handled in the dashboard interface + + +// TODOs +// - [x] make the controllers inherit from the base controller class +// - [x] port TorqueControllerSimple +// - [x] port TorqueControllerLoadCellVectoring +// - [x] port BaseLaunchController +// - [x] port TorqueControllerSimpleLaunch +// - [x] port slip launch +// - [x] port TorqueControllerLookupLaunch +// - [x] port CASE +// - [x] add the torque limit evaluation logic into dashboard interface +// - [x] integrate into state machine +// - [x] pass through the car state +// - [x] get dial and torque mode from the dashboard +// - [x] create car_state in main and pass into state machine +// - [x] add 3 bit status to a telemetry message for the TC mux status to HT_CAN +// - [x] pass state of tc mux into telem interface and add the CAN signal +// - [x] remove the old tc mux +// - [ ] write integration tests for the real controllers +// - [x] test construction with real controllers +// - [ ] ensure that sane outputs occur on first tick of each controller +// - [x] update the state machine unit test with integration test of new tc mux + +// ON CAR testing +// - [ ] test the change of the torque mode from the dashboard interface +// - [ ] write testing code for this in separate environment + + +namespace TC_MUX_DEFAULT_PARAMS +{ + const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s + const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm + const float MAX_POWER_LIMIT = 630000.0; +}; -#include -#include -#include "TorqueControllers.h" -#include "DrivetrainSystem.h" -#include "PedalsSystem.h" -#include "SteeringSystem.h" -#include "DashboardInterface.h" -#include "VectornavInterface.h" -#include "LoadCellInterface.h" +// namespace TC_MUX +// { -const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s -const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm -/// @brief +template class TorqueControllerMux { -private: - TorqueControllerNone torqueControllerNone_; - TorqueControllerSimple torqueControllerSimple_; - TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; - TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_; - TorqueControllerSlipLaunch torqueControllerSlipLaunch_; - TorqueControllerCASEWrapper tcCASEWrapper_; - - // Use this to map the dial to TCMUX modes - std::unordered_map dialModeMap_ = { - {ControllerMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, - {ControllerMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING}, - {ControllerMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM}, - {ControllerMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH}, - {ControllerMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH}, - {ControllerMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, - }; - std::unordered_map torqueLimitMap_ = { - {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0}, - {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, - {TorqueLimit_e::TCMUX_FULL_TORQUE, AMK_MAX_TORQUE} - }; - - TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; - ControllerMode_e cur_dial_mode_ = ControllerMode_e::MODE_0; - - TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; + static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); - TorqueControllerBase* controllers[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)] = { - static_cast(&torqueControllerNone_), - static_cast(&torqueControllerSimple_), - static_cast(&torqueControllerLoadCellVectoring_), - static_cast(&torqueControllerSimpleLaunch_), - static_cast(&torqueControllerSlipLaunch_), - static_cast(&tcCASEWrapper_) - }; - - DrivetrainCommand_s drivetrainCommand_; - TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE; - bool torqueLimitButtonPressed_ = false; - unsigned long torqueLimitButtonPressedTime_ = 0; +private: + std::array controller_pointers_; + + std::array mux_bypass_limits_; + + std::unordered_map torque_limit_map_ = { + {TorqueLimit_e::TCMUX_FULL_TORQUE, PhysicalParameters::AMK_MAX_TORQUE}, + {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0f}, + {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, + DrivetrainCommand_s previous_controller_command, + DrivetrainCommand_s desired_controller_out); + + DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); + DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); + DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); + DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); + public: - /// @brief torque controller mux in which default instances of all torque controllers are created for use - TorqueControllerMux() - : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) - , 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)]) - , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} - - - /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC - /// @param simpleTCRearTorqueScale the scaling from 0 to 2 in which 2 is full rear torque allocation, 0 is full front, 1 = balanced - /// @param simpleTCRegenTorqueScale scaling from 0 to 2 in which 0 is full rear regen and 2 is full front regen, 1 = balanced - TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale) - : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) - , 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)]) - , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} -// Functions - void tick( - const SysTick_s &tick, - const DrivetrainDynamicReport_s &drivetrainData, - const PedalsSystemData_s &pedalsData, - const SteeringSystemData_s &steeringData, - const LoadCellInterfaceOutput_s &loadCellData, - ControllerMode_e dashboardDialMode, - bool dashboardTorqueModeButtonPressed, - const vectornav &vn_data, - const DrivetrainCommand_s &CASECommand - ); - const DrivetrainCommand_s &getDrivetrainCommand() - { - return drivetrainCommand_; - }; - const TorqueLimit_e &getTorqueLimit() - { - return torqueLimit_; - }; - - const float getMaxTorque() - { - return torqueLimitMap_[torqueLimit_]; - } - - void applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); + TorqueControllerMux() = delete; + /// @brief constructor for the TC mux + /// @param controller_pointers the array of pointers to the controllers being muxed between + /// @param mux_bypass_limits the array of aligned bools for determining if the limits should be applied to the controller outputs defaults to TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + /// @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 + explicit TorqueControllerMux(std::array controller_pointers, + std::array mux_bypass_limits, + float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, + float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE, + float max_power_limit = TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT) : controller_pointers_(controller_pointers), + mux_bypass_limits_(mux_bypass_limits), + max_change_speed_(max_change_speed), + max_torque_pos_change_delta_(max_torque_pos_change_delta), + max_power_limit_(max_power_limit) - void applyTorqueLimit(DrivetrainCommand_s* command); - void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); - - void applyPosSpeedLimit(DrivetrainCommand_s* command); - - - - const ControllerMode_e getDialMode() { - return cur_dial_mode_; - } + static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); - const TorqueController_e getDriveMode() - { - return muxMode_; } - TorqueControllerBase* activeController() - { - // check to make sure that there is actually a controller - // at the muxMode_ idx - if (controllers[muxMode_] != NULL) { - return controllers[muxMode_]; - } else { - return static_cast(&torqueControllerNone_); - } - } -}; + const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } -#endif /* __TORQUECTRLMUX_H__ */ + /// @brief function that evaluates the mux, controllers and gets the current 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 + DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e controller_command_torque_limit, + const car_state &input_state); +}; +// } +#include "TorqueControllerMux.tpp" +#endif // __TorqueControllerMux_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMuxV2.tpp b/lib/systems/include/TorqueControllerMux.tpp similarity index 85% rename from lib/systems/include/TorqueControllerMuxV2.tpp rename to lib/systems/include/TorqueControllerMux.tpp index bf320dd2c..7b4cdd2da 100644 --- a/lib/systems/include/TorqueControllerMuxV2.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -1,9 +1,9 @@ -#include "TorqueControllerMuxV2.h" +#include "TorqueControllerMux.h" template -DrivetrainCommand_s TorqueControllerMuxv2::getDrivetrainCommand(ControllerMode_e requested_controller_type, - TorqueLimit_e requested_torque_limit, - const car_state &input_state) +DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e requested_torque_limit, + const car_state &input_state) { TorqueControllerOutput_s current_output; @@ -34,9 +34,9 @@ DrivetrainCommand_s TorqueControllerMuxv2::getDrivetrainCommand } template -TorqueControllerMuxError TorqueControllerMuxv2::can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, - DrivetrainCommand_s previous_controller_command, - DrivetrainCommand_s desired_controller_out) +TorqueControllerMuxError TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + DrivetrainCommand_s previous_controller_command, + DrivetrainCommand_s desired_controller_out) { bool speedPreventsModeChange = false; // Check if torque delta permits mode change @@ -60,7 +60,7 @@ TorqueControllerMuxError TorqueControllerMuxv2::can_switch_cont /* Apply limit such that wheelspeed never goes negative */ template -DrivetrainCommand_s TorqueControllerMuxv2::apply_positive_speed_limit_(const DrivetrainCommand_s &command) +DrivetrainCommand_s TorqueControllerMux::apply_positive_speed_limit_(const DrivetrainCommand_s &command) { DrivetrainCommand_s out; out = command; @@ -72,7 +72,7 @@ DrivetrainCommand_s TorqueControllerMuxv2::apply_positive_speed } template -DrivetrainCommand_s TorqueControllerMuxv2::apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque) +DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque) { DrivetrainCommand_s out = command; float avg_torque = 0; @@ -104,7 +104,7 @@ DrivetrainCommand_s TorqueControllerMuxv2::apply_torque_limit_( preserve functionality of torque controllers */ template -DrivetrainCommand_s TorqueControllerMuxv2::apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque) +DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque) { DrivetrainCommand_s out = command; float net_torque_mag = 0; @@ -143,7 +143,7 @@ DrivetrainCommand_s TorqueControllerMuxv2::apply_power_limit_(c } template -DrivetrainCommand_s TorqueControllerMuxv2::apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data) +DrivetrainCommand_s TorqueControllerMux::apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data) { DrivetrainCommand_s out = command; const float noRegenLimitKPH = 10.0; diff --git a/lib/systems/include/TorqueControllerMuxV2.h b/lib/systems/include/TorqueControllerMuxV2.h deleted file mode 100644 index 2e13261b6..000000000 --- a/lib/systems/include/TorqueControllerMuxV2.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef TORQUECONTROLLERMUX -#define TORQUECONTROLLERMUX - -#include -#include "SharedDataTypes.h" -#include "BaseController.h" - -// notes: -// 21 torque limit should be first - -// tc mux needs to handle these things: -// 1 swapping between controller outputs -// 2 turning on and off running of controllers -// 3 application of safeties and limits to controller outputs -// 4 torque limit changing (torque mode) --> -// TODO the torque limit value changing should be handled in the dashboard interface - - -// TODOs -// - [-] make the controllers inherit from the base controller class -// - [x] port TorqueControllerSimple -// - [x] port TorqueControllerLoadCellVectoring -// - [x] port BaseLaunchController -// - [x] port TorqueControllerSimpleLaunch -// - [x] port slip launch -// - [x] port TorqueControllerLookupLaunch -// - [ ] port CASE -// - [ ] integrate into state machine -// - [ ] set the car_state from all sources -// - [ ] get dial and torque mode from the dashboard -// - [ ] write integration test for the real controllers -// - [ ] update the state machine unit test with integration test of new tc mux - - - -namespace TC_MUX_DEFAULT_PARAMS -{ - const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s - const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm - const float MAX_POWER_LIMIT = 630000.0; -}; - - -// namespace TC_MUX -// { - - -template -class TorqueControllerMuxv2 -{ - static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); - - - -private: - std::array controller_pointers_; - - std::array mux_bypass_limits_; - - std::unordered_map torque_limit_map_ = { - {TorqueLimit_e::TCMUX_LOW_TORQUE, AMK_MAX_TORQUE}, - {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, - {TorqueLimit_e::TCMUX_FULL_TORQUE, 10.0}}; - 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, - DrivetrainCommand_s previous_controller_command, - DrivetrainCommand_s desired_controller_out); - - DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); - DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); - DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); - DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); - -public: - - TorqueControllerMuxv2() = delete; - /// @brief constructor for the TC mux - /// @param controller_pointers the array of pointers to the controllers being muxed between - /// @param mux_bypass_limits the array of aligned bools for determining if the limits should be applied to the controller outputs defaults to TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE - /// @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 - explicit TorqueControllerMuxv2(std::array controller_pointers, - std::array mux_bypass_limits, - float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, - float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE, - float max_power_limit = TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT) : controller_pointers_(controller_pointers), - mux_bypass_limits_(mux_bypass_limits), - max_change_speed_(max_change_speed), - max_torque_pos_change_delta_(max_torque_pos_change_delta), - max_power_limit_(max_power_limit) - - - { - static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); - - } - - const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } - - /// @brief function that evaluates the mux, controllers and gets the current 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 - DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, - TorqueLimit_e controller_command_torque_limit, - const car_state &input_state); -}; -// } -#include "TorqueControllerMuxV2.tpp" -#endif // __TORQUECONTROLLERMUXV2_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 3909b498b..0194e82d5 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -1,311 +1,7 @@ -#ifndef __TORQUECONTROLLERS_H__ -#define __TORQUECONTROLLERS_H__ - -#include -#include -#include -#include -#include "AnalogSensorsInterface.h" -#include "DashboardInterface.h" -#include "PhysicalParameters.h" -#include "VectornavInterface.h" -#include "SteeringSystem.h" -#include "LoadCellInterface.h" - -#include "accel_lookup.h" - -#include "SharedDataTypes.h" - -/* CONTROLLER CONSTANTS */ - -const float MAX_POWER_LIMIT = 63000.0; // max mechanical power limit in KW - -/* MOTOR CONSTANTS */ - -const float AMK_MAX_RPM = 20000; -// 10MPH LIMIT for lot testing lmao -// const float AMK_MAX_RPM = (13.4 * METERS_PER_SECOND_TO_RPM); // 30mph -// const float AMK_MAX_RPM = (4.47 * METERS_PER_SECOND_TO_RPM); // 10mph -// const float AMK_MAX_RPM = (2.235 * METERS_PER_SECOND_TO_RPM); // 5mph -// const float AMK_MAX_RPM = (.89 * METERS_PER_SECOND_TO_RPM); // 1mph -// const float -const float AMK_MAX_TORQUE = 21.42; -const float MAX_REGEN_TORQUE = 10.0; - -const float const_accel_time = 100; // time to use launch speed target in ms - -const float launch_ready_accel_threshold = .1; -const float launch_ready_brake_threshold = .2; -const float launch_ready_speed_threshold = 5.0 * METERS_PER_SECOND_TO_RPM; // rpm -const float launch_go_accel_threshold = .9; -const float launch_stop_accel_threshold = .5; - -/* DRIVETRAIN STRUCTS */ - - - - - -/* TC STRUCTS */ -struct TCCaseWrapperTick_s -{ - const DrivetrainCommand_s &command; - const SteeringSystemData_s &steeringData; -}; - -/* ENUMS */ - -enum TorqueController_e -{ - TC_NO_CONTROLLER = 0, - 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_NUM_CONTROLLERS = 7, -}; - -/* TORQUE CONTROLLERS */ - -/* - Base torque controller to allow access to internal torque controller members -*/ -class TorqueControllerBase -{ -public: - /* returns the launch state for the purpose of lighting the dahsboard LED and unit testing. To be overridden in launch torque modes */ - virtual LaunchStates_e get_launch_state() { return LaunchStates_e::NO_LAUNCH_MODE; } -}; - -template -class TorqueController : public TorqueControllerBase -{ -protected: - -public: -}; - -class TorqueControllerNone : public TorqueController -{ -private: -public: - TorqueControllerNone(TorqueControllerOutput_s &writeout) - { - writeout.command = TC_COMMAND_NO_TORQUE; - writeout.ready = true; - }; -}; - -/// @brief Simple torque controller that only considers pedal inputs and torque limit -class TorqueControllerSimple : public TorqueController -{ -private: - TorqueControllerOutput_s &writeout_; - float frontTorqueScale_ = 1.0; - float rearTorqueScale_ = 1.0; - float frontRegenTorqueScale_ = 1.0; - float rearRegenTorqueScale_ = 1.0; - -public: - /// @brief simple TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance - /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command - /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD - /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear - TorqueControllerSimple(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), - rearTorqueScale_(rearTorqueScale), - frontRegenTorqueScale_(2.0 - regenTorqueScale), - rearRegenTorqueScale_(regenTorqueScale) - { - writeout_.command = TC_COMMAND_NO_TORQUE; - writeout_.ready = true; - } - TorqueControllerSimple(TorqueControllerOutput_s &writeout) : TorqueControllerSimple(writeout, 1.0, 1.0) {} - - void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit); -}; - -class TorqueControllerLoadCellVectoring : public TorqueController -{ -private: - TorqueControllerOutput_s &writeout_; - float frontTorqueScale_ = 1.0; - float rearTorqueScale_ = 1.0; - float frontRegenTorqueScale_ = 1.0; - float rearRegenTorqueScale_ = 1.0; - /* - FIR filter designed with - http://t-filter.appspot.com - - sampling frequency: 100 Hz - - * 0 Hz - 10 Hz - gain = 1 - desired ripple = 5 dB - actual ripple = 1.7659949026015025 dB - - * 40 Hz - 50 Hz - gain = 0 - desired attenuation = -40 dB - actual attenuation = -47.34009380570117 dB - */ - const static int numFIRTaps_ = 5; - float FIRTaps_[numFIRTaps_] = { - 0.07022690881526232, - 0.27638313122745306, - 0.408090001549378, - 0.27638313122745306, - 0.07022690881526232}; - int FIRCircBufferHead = 0; // index of the latest sample in the raw buffer - float loadCellForcesRaw_[4][numFIRTaps_] = {}; - float loadCellForcesFiltered_[4] = {}; - // Some checks that can disable the controller - const int errorCountThreshold_ = 25; - int loadCellsErrorCounter_[4] = {}; - bool FIRSaturated_ = false; - bool ready_ = false; - -public: - /// @brief load cell TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance - /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command - /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD - /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear - TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), - rearTorqueScale_(rearTorqueScale), - frontRegenTorqueScale_(2.0 - regenTorqueScale), - rearRegenTorqueScale_(regenTorqueScale) - { - writeout_.command = TC_COMMAND_NO_TORQUE; - writeout_.ready = false; - } - TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0, 1.0) {} - - void tick( - const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - float torqueLimit, - const LoadCellInterfaceOutput_s &loadCellData - ); -}; - -class BaseLaunchController -{ -protected: - TorqueControllerOutput_s &writeout_; - uint32_t time_of_launch_; - double initial_ecef_x_; - double initial_ecef_y_; - double initial_ecef_z_; - LaunchStates_e launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; - uint32_t current_millis_; - float launch_speed_target_ = 0.0; - int16_t init_speed_target_ = 0.0; -public: - BaseLaunchController(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) - : writeout_(writeout), - init_speed_target_(initial_speed_target) - { - writeout.command = TC_COMMAND_NO_TORQUE; - writeout_.ready = true; - } - - void tick(const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - const float wheel_rpms[], - const vectornav *vn_data); - - virtual void calc_launch_algo(const vectornav *vn_data) = 0; -}; - -class TorqueControllerSimpleLaunch : public TorqueController, public BaseLaunchController -{ -private: - float launch_rate_target_; - -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 - @param launch_rate specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout, float launch_rate, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target), - launch_rate_target_(launch_rate) {} - - TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSimpleLaunch(writeout, DEFAULT_LAUNCH_RATE, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vectornav *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 vectornav *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 vectornav *vn_data) override; -}; - -class TorqueControllerCASEWrapper : public TorqueController -{ -public: - void tick(const TCCaseWrapperTick_s &intake); - TorqueControllerCASEWrapper(TorqueControllerOutput_s &writeout) : writeout_(writeout) - { - writeout_ = writeout; - } - -private: - TorqueControllerOutput_s &writeout_; -}; - -#endif /* __TORQUECONTROLLERS_H__ */ +//controllers +#include "CASEControllerWrapper.h" +#include "LoadCellVectoringController.h" +#include "LookupLaunchController.h" +#include "SimpleLaunchController.h" +#include "SimpleController.h" +#include "SlipLaunchController.h" \ No newline at end of file diff --git a/lib/systems/src/BaseLaunchController.cpp b/lib/systems/src/BaseLaunchController.cpp index 113980538..7871c9e4c 100644 --- a/lib/systems/src/BaseLaunchController.cpp +++ b/lib/systems/src/BaseLaunchController.cpp @@ -1,5 +1,5 @@ #include "BaseLaunchController.h" - +#include /* abs */ void BaseLaunchController::tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, @@ -12,7 +12,7 @@ void BaseLaunchController::tick( current_millis_ = tick.millis; - int16_t brake_torque_req = pedalsData.regenPercent * MAX_REGEN_TORQUE; + int16_t brake_torque_req = pedalsData.regenPercent * PhysicalParameters::MAX_REGEN_TORQUE; float max_speed = 0; for (int i = 0; i < 4; i++) @@ -40,7 +40,7 @@ void BaseLaunchController::tick( launch_speed_target_ = 0; time_of_launch_ = tick.millis; // check speed is 0 and pedals not pressed - if ((pedalsData.accelPercent < launch_ready_accel_threshold) && (pedalsData.brakePercent < launch_ready_brake_threshold) && (max_speed < launch_ready_speed_threshold)) + if ((pedalsData.accelPercent < BaseLaunchControllerParams::launch_ready_accel_threshold) && (pedalsData.brakePercent < BaseLaunchControllerParams::launch_ready_brake_threshold) && (max_speed < BaseLaunchControllerParams::launch_ready_speed_threshold)) { launch_state_ = LaunchStates_e::LAUNCH_READY; } @@ -63,11 +63,11 @@ void BaseLaunchController::tick( time_of_launch_ = current_millis_; // check speed is 0 and brake not pressed - if ((pedalsData.brakePercent >= launch_ready_brake_threshold) || (max_speed >= launch_ready_speed_threshold)) + if ((pedalsData.brakePercent >= BaseLaunchControllerParams::launch_ready_brake_threshold) || (max_speed >= BaseLaunchControllerParams::launch_ready_speed_threshold)) { launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; } - else if (pedalsData.accelPercent >= launch_go_accel_threshold) + else if (pedalsData.accelPercent >= BaseLaunchControllerParams::launch_go_accel_threshold) { initial_ecef_x_ = vn_data.ecef_coords[0]; @@ -82,7 +82,7 @@ void BaseLaunchController::tick( case LaunchStates_e::LAUNCHING: { // use brackets to ignore 'cross initialization' of secs_since_launch // check accel below launch threshold and brake above - if ((pedalsData.accelPercent <= launch_stop_accel_threshold) || (pedalsData.brakePercent >= launch_ready_brake_threshold)) + if ((pedalsData.accelPercent <= BaseLaunchControllerParams::launch_stop_accel_threshold) || (pedalsData.brakePercent >= BaseLaunchControllerParams::launch_ready_brake_threshold)) { launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; } @@ -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] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[FR] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RL] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RR] = AMK_MAX_TORQUE; + 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; } break; default: diff --git a/lib/systems/src/LaunchControllerAlgos.cpp b/lib/systems/src/LaunchControllerAlgos.cpp index 3aadd062b..d5a107006 100644 --- a/lib/systems/src/LaunchControllerAlgos.cpp +++ b/lib/systems/src/LaunchControllerAlgos.cpp @@ -2,6 +2,7 @@ #include "SlipLaunchController.h" #include "SimpleLaunchController.h" + void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav &vn_data) { /* @@ -14,7 +15,7 @@ void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav &vn_data) float secs_since_launch = (float)(current_millis_ - time_of_launch_) / 1000.0; launch_speed_target_ = (int16_t)((float)secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); launch_speed_target_ += init_speed_target_; - launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); + launch_speed_target_ = std::min((int)PhysicalParameters::AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); } void TorqueControllerLookupLaunch::calc_launch_algo(const vectornav &vn_data) diff --git a/lib/systems/src/LoadCellVectoringController.cpp b/lib/systems/src/LoadCellVectoringController.cpp index ebbec934d..e92241e92 100644 --- a/lib/systems/src/LoadCellVectoringController.cpp +++ b/lib/systems/src/LoadCellVectoringController.cpp @@ -57,12 +57,12 @@ void TorqueControllerLoadCellVectoring::tick( { // Positive torque request // NOTE: using "torquePool" here instead of torqueRequest for legibility - torquePool = accelRequest * AMK_MAX_TORQUE * 4; + torquePool = accelRequest * PhysicalParameters::AMK_MAX_TORQUE * 4; - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[FL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = PhysicalParameters::AMK_MAX_RPM; + 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; @@ -73,7 +73,7 @@ void TorqueControllerLoadCellVectoring::tick( { // Negative torque request // No load cell vectoring on regen - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + torqueRequest = PhysicalParameters::MAX_REGEN_TORQUE * accelRequest * -1.0; writeout_.command.speeds_rpm[FL] = 0.0; writeout_.command.speeds_rpm[FR] = 0.0; @@ -88,7 +88,7 @@ void TorqueControllerLoadCellVectoring::tick( } else { - writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; } } } diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/SimpleController.cpp index a9151ad6d..865d51555 100644 --- a/lib/systems/src/SimpleController.cpp +++ b/lib/systems/src/SimpleController.cpp @@ -14,16 +14,16 @@ void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_ if (accelRequest >= 0.0) { // Positive torque request - torqueRequest = accelRequest * AMK_MAX_TORQUE; + torqueRequest = accelRequest * PhysicalParameters::AMK_MAX_TORQUE; - // writeout_.command.speeds_rpm[FL] = accelRequest * AMK_MAX_RPM; - // writeout_.command.speeds_rpm[FR] = accelRequest * AMK_MAX_RPM;pid_input_ - // writeout_.command.speeds_rpm[RL] = accelRequest * AMK_MAX_RPM; - // writeout_.command.speeds_rpm[RR] = accelRequest * AMK_MAX_RPM; - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; + // writeout_.command.speeds_rpm[FL] = accelRequest * PhysicalParameters::AMK_MAX_RPM; + // writeout_.command.speeds_rpm[FR] = accelRequest * PhysicalParameters::AMK_MAX_RPM;pid_input_ + // writeout_.command.speeds_rpm[RL] = accelRequest * PhysicalParameters::AMK_MAX_RPM; + // writeout_.command.speeds_rpm[RR] = accelRequest * PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[FL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = PhysicalParameters::AMK_MAX_RPM; + 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_; @@ -33,7 +33,7 @@ void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_ else { // Negative torque request - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + torqueRequest = PhysicalParameters::MAX_REGEN_TORQUE * accelRequest * -1.0; writeout_.command.speeds_rpm[FL] = 0.0; writeout_.command.speeds_rpm[FR] = 0.0; diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp deleted file mode 100644 index 0d4e28b58..000000000 --- a/lib/systems/src/TorqueControllerMux.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "TorqueControllerMux.h" -#include "Utility.h" -#include "PhysicalParameters.h" - -void TorqueControllerMux::tick( - const SysTick_s &tick, - const DrivetrainDynamicReport_s &drivetrainData, - const PedalsSystemData_s &pedalsData, - const SteeringSystemData_s &steeringData, - const LoadCellInterfaceOutput_s &loadCellData, - ControllerMode_e dashboardDialMode, - bool dashboardTorqueModeButtonPressed, - const vectornav &vn_data, - const DrivetrainCommand_s &CASECommand) -{ - // 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); - tcCASEWrapper_.tick( - (TCCaseWrapperTick_s){ - .command = CASECommand, - .steeringData = steeringData}); - - // Tick torque button logic at 50hz - if (tick.triggers.trigger50) - { - // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms - if ( - torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) - { - // WOW C++ is ass - torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); - torqueLimitButtonPressedTime_ = tick.millis; - } - torqueLimitButtonPressed_ = dashboardTorqueModeButtonPressed; - } - - // Tick TCMUX at 100hz - if (tick.triggers.trigger100) - { - // Determine next state based on dashboard dial state - // Only transisiton to the next state if: - // Vehicle speed is below 5 m/s - // AND torque delta between old and new controllers is < 0.5nm on every wheel - // AND the selected torque controller is in the ready state - if (muxMode_ != dialModeMap_[dashboardDialMode]) - { - // Check if speed permits mode change - bool speedPreventsModeChange = false; - for (int i = 0; i < NUM_MOTORS; i++) - { - speedPreventsModeChange |= drivetrainData.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND >= MAX_SPEED_FOR_MODE_CHANGE; - } - - // Check if torque delta permits mode change - bool torqueDeltaPreventsModeChange = false; - for (int i = 0; i < NUM_MOTORS; i++) - { - float torqueDelta = abs( - controllerOutputs_[static_cast(muxMode_)].command.torqueSetpoints[i] - controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].command.torqueSetpoints[i]); - - if (torqueDelta > MAX_TORQUE_DELTA_FOR_MODE_CHANGE) - { - torqueDeltaPreventsModeChange = true; - break; - } - } - - // Check if targeted controller is ready to be selected - bool controllerNotReadyPreventsModeChange = (controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].ready == false); - - // if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) - // { - muxMode_ = dialModeMap_[dashboardDialMode]; - cur_dial_mode_ = dashboardDialMode; - // } - } - - // Check if the current controller is ready. If it has faulted, revert to safe mode - // When the car goes below 5m/s, it will attempt to re-engage the faulted controller - // It will stay in safe mode if the controller is still faulted - if (controllerOutputs_[static_cast(muxMode_)].ready == false) - { - muxMode_ = TorqueController_e::TC_SAFE_MODE; - } - - drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; - - // apply torque limit before power limit to not power limit - // applyRegenLimit(&drivetrainCommand_, &drivetrainData); - // applyTorqueLimit(&drivetrainCommand_); - // applyPowerLimit(&drivetrainCommand_, &drivetrainData); - applyPosSpeedLimit(&drivetrainCommand_); - } -} - -/* - Apply limit to make sure that regenerative braking is not applied when - wheelspeed is below 5kph on all wheels. -*/ -void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) -{ - const float noRegenLimitKPH = 10.0; - const float fullRegenLimitKPH = 5.0; - float maxWheelSpeed = 0.0; - float torqueScaleDown = 0.0; - bool allWheelsRegen = true; // true when all wheels are targeting speeds below the current wheel speed - - for (int i = 0; i < NUM_MOTORS; i++) - { -#ifdef ARDUINO_TEENSY41 - maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); - allWheelsRegen &= (command->speeds_rpm[i] < abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); -#else - maxWheelSpeed = std::max(maxWheelSpeed, std::abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); - allWheelsRegen &= (command->speeds_rpm[i] < std::abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); -#endif - } - - // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH - // linearly interpolate the scale factor between noRegenLimitKPH and fullRegenLimitKPH - torqueScaleDown = std::min(1.0f, std::max(0.0f, (maxWheelSpeed - fullRegenLimitKPH) / (noRegenLimitKPH - fullRegenLimitKPH))); - - if (allWheelsRegen) - { - for (int i = 0; i < NUM_MOTORS; i++) - { - command->torqueSetpoints[i] *= torqueScaleDown; - } - } -} - -/* - Apply power limit 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 -*/ -void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) -{ - float net_torque_mag = 0; - float net_power = 0; - - // calculate current mechanical power - for (int i = 0; i < NUM_MOTORS; i++) - { -// get the total magnitude of torque from all 4 wheels -#ifdef ARDUINO_TEENSY41 // screw arduino.h macros - net_torque_mag += abs(command->torqueSetpoints[i]); - net_power += abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#else - // sum up net torque - net_torque_mag += std::abs(command->torqueSetpoints[i]); - // calculate P = T*w for each wheel and sum together - net_power += std::abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#endif - } - - // only evaluate power limit if current power exceeds it - if (net_power > (MAX_POWER_LIMIT)) - { - for (int i = 0; i < NUM_MOTORS; i++) - { - - // TODO: SOMEBODY PLZ MAKE THIS WORK - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); - - // calculate the percent of total torque requested per wheel - float torque_percent = abs(command->torqueSetpoints[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 * MAX_POWER_LIMIT); - -// power / omega (motor rad/s) to get torque per wheel -#ifdef ARDUINO_TEENSY41 - command->torqueSetpoints[i] = abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#else - command->torqueSetpoints[i] = std::abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#endif - command->torqueSetpoints[i] = std::max(0.0f, std::min(command->torqueSetpoints[i], getMaxTorque())); - } - } -} - -/* - Apply limit such that the average wheel torque is never above the max - torque allowed in the current torque mode. - This will uniformally scale down all torques as to not affect the functionality - of non-symmetrical torque controllers. -*/ -void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s *command) -{ - float max_torque = getMaxTorque(); - float avg_torque = 0; - - // get the average torque accross all 4 wheels - for (int i = 0; i < NUM_MOTORS; i++) - { -#ifdef ARDUINO_TEENSY41 // screw arduino.h macros - avg_torque += abs(command->torqueSetpoints[i]); -#else - avg_torque += std::abs(command->torqueSetpoints[i]); -#endif - } - avg_torque /= NUM_MOTORS; - - // if this is greather than the torque limit, scale down - if (avg_torque > max_torque) - { - // get the scale of avg torque above max torque - float scale = avg_torque / max_torque; - // divide by scale to lower avg below max torque - for (int i = 0; i < NUM_MOTORS; i++) - { - command->torqueSetpoints[i] /= scale; - } - } -} - -/* Apply limit such that wheelspeed never goes negative */ -void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command) -{ - for (int i = 0; i < NUM_MOTORS; i++) - { - command->speeds_rpm[i] = std::max(0.0f, command->speeds_rpm[i]); - } -} \ No newline at end of file diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp deleted file mode 100644 index d12adeb2e..000000000 --- a/lib/systems/src/TorqueControllers.cpp +++ /dev/null @@ -1,332 +0,0 @@ -#include "TorqueControllers.h" -#include "Utility.h" -#include -#include "PhysicalParameters.h" - -// TorqueControllerSimple - -void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit) -{ - - // Calculate torque commands at 100hz - if (tick.triggers.trigger100) - { - // Both pedals are not pressed and no implausibility has been detected - // accelRequest goes between 1.0 and -1.0 - float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; - float torqueRequest; - - if (accelRequest >= 0.0) - { - // Positive torque request - torqueRequest = accelRequest * AMK_MAX_TORQUE; - - // writeout_.command.speeds_rpm[FL] = accelRequest * AMK_MAX_RPM; - // writeout_.command.speeds_rpm[FR] = accelRequest * AMK_MAX_RPM;pid_input_ - // writeout_.command.speeds_rpm[RL] = accelRequest * AMK_MAX_RPM; - // writeout_.command.speeds_rpm[RR] = accelRequest * AMK_MAX_RPM; - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = 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_; - } - else - { - // Negative torque request - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; - - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - 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_; - } - } -} - -// TorqueControllerLoadCellVectoring - -void TorqueControllerLoadCellVectoring::tick( - const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - float torqueLimit, - const LoadCellInterfaceOutput_s &loadCellData -) -{ - // Calculate torque commands at 100hz - if (tick.triggers.trigger100) - { - // Apply FIR filter to load cell data - loadCellForcesRaw_[0][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FL; - loadCellForcesRaw_[1][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FR; - loadCellForcesRaw_[2][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RL; - loadCellForcesRaw_[3][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RR; - - for (int i = 0; i < 4; i++) - { - loadCellForcesFiltered_[i] = 0.0f; - for (int FIROffset = 0; FIROffset < numFIRTaps_; FIROffset++) - { - int index = (FIRCircBufferHead + FIROffset) % numFIRTaps_; - loadCellForcesFiltered_[i] += loadCellForcesRaw_[i][index] * FIRTaps_[FIROffset]; - } - } - FIRCircBufferHead = (FIRCircBufferHead + 1) % numFIRTaps_; - if (FIRCircBufferHead == numFIRTaps_ - 1) - FIRSaturated_ = true; - - // Do sanity checks on raw data - loadCellsErrorCounter_[0] = loadCellData.loadCellConversions.FL.raw != 4095 && loadCellData.loadCellConversions.FL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; - loadCellsErrorCounter_[1] = loadCellData.loadCellConversions.FR.raw != 4095 && loadCellData.loadCellConversions.FR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; - loadCellsErrorCounter_[2] = loadCellData.loadCellConversions.RL.raw != 4095 && loadCellData.loadCellConversions.RL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; - loadCellsErrorCounter_[3] = loadCellData.loadCellConversions.RR.raw != 4095 && loadCellData.loadCellConversions.RR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; - ready_ = FIRSaturated_ && loadCellsErrorCounter_[0] < errorCountThreshold_ && loadCellsErrorCounter_[1] < errorCountThreshold_ && loadCellsErrorCounter_[2] < errorCountThreshold_ && loadCellsErrorCounter_[3] < errorCountThreshold_; - - writeout_.ready = ready_; - - if (ready_) - { - // Calculate total normal force - float sumNormalForce = 0.0f; - for (int i = 0; i < 4; i++) - { - sumNormalForce += loadCellForcesFiltered_[i]; - } - - // Both pedals are not pressed and no implausibility has been detected - // accelRequest goes between 1.0 and -1.0 - float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; - float torquePool; - float torqueRequest; - - if (accelRequest >= 0.0) - { - // Positive torque request - // NOTE: using "torquePool" here instead of torqueRequest for legibility - torquePool = accelRequest * AMK_MAX_TORQUE * 4; - - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = 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; - } - else - { - // Negative torque request - // No load cell vectoring on regen - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; - - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - 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_; - } - } - else - { - writeout_.command = TC_COMMAND_NO_TORQUE; - } - } -} - -void BaseLaunchController::tick( - const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - const float wheel_rpms[], - const vectornav* vn_data) -{ - - if (tick.triggers.trigger100){ - - current_millis_ = tick.millis; - - int16_t brake_torque_req = pedalsData.regenPercent * MAX_REGEN_TORQUE; - - float max_speed = 0; - for(int i = 0; i < 4; i++){ - max_speed = std::max(max_speed, abs(wheel_rpms[i])); - } - - writeout_.ready = true; - - switch(launch_state_){ - case LaunchStates_e::LAUNCH_NOT_READY: - // set torques and speed to 0 - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - 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; - - //init launch vars - launch_speed_target_ = 0; - time_of_launch_ = tick.millis; - // check speed is 0 and pedals not pressed - if((pedalsData.accelPercent < launch_ready_accel_threshold) - && (pedalsData.brakePercent < launch_ready_brake_threshold) - && (max_speed < launch_ready_speed_threshold)) - { - launch_state_ = LaunchStates_e::LAUNCH_READY; - } - - break; - case LaunchStates_e::LAUNCH_READY: - // set torques and speed to 0 - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - 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; - - //init launch vars - launch_speed_target_ = 0; - time_of_launch_ = current_millis_; - - //check speed is 0 and brake not pressed - if ((pedalsData.brakePercent >= launch_ready_brake_threshold) - || (max_speed >= launch_ready_speed_threshold)) - { - launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; - } else if(pedalsData.accelPercent >= launch_go_accel_threshold){ - - initial_ecef_x_ = vn_data->ecef_coords[0]; - initial_ecef_y_ = vn_data->ecef_coords[1]; - initial_ecef_z_ = vn_data->ecef_coords[2]; - - launch_state_ = LaunchStates_e::LAUNCHING; - } - - //check accel above launch threshold and launch - break; - case LaunchStates_e::LAUNCHING: - { // use brackets to ignore 'cross initialization' of secs_since_launch - //check accel below launch threshold and brake above - if((pedalsData.accelPercent <= launch_stop_accel_threshold) - || (pedalsData.brakePercent >= launch_ready_brake_threshold)) - { - launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; - } - - calc_launch_algo(vn_data); - - writeout_.command.speeds_rpm[FL] = launch_speed_target_; - writeout_.command.speeds_rpm[FR] = launch_speed_target_; - writeout_.command.speeds_rpm[RL] = launch_speed_target_; - writeout_.command.speeds_rpm[RR] = launch_speed_target_; - - writeout_.command.torqueSetpoints[FL] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[FR] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RL] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RR] = AMK_MAX_TORQUE; - - } - break; - default: - break; - - } - - } -} - -void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav* vn_data) { - /* - Stolen launch algo from HT07. This ramps up the speed target over time. - launch rate target is m/s^2 and is the target acceleration rate - secs_since_launch takes the milliseconds since launch started and converts to sec - This is then converted to RPM for a speed target - There is an initial speed target that is your iitial instant acceleration on the wheels - */ - float secs_since_launch = (float)(current_millis_ - time_of_launch_) / 1000.0; - launch_speed_target_ = (int16_t)((float) secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); - launch_speed_target_ += init_speed_target_; - launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); -} - -void TorqueControllerSlipLaunch::calc_launch_algo(const vectornav* 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 vectornav* 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) -{ - - for (int i = 0; i < NUM_MOTORS; i++) - { - writeout_.command.speeds_rpm[i] = intake.command.speeds_rpm[i]; - writeout_.command.torqueSetpoints[i] = intake.command.torqueSetpoints[i]; - } - - writeout_.ready = intake.steeringData.status != SteeringSystemStatus_e::STEERING_SYSTEM_ERROR; - -} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index c83ff77a2..f981fa427 100644 --- a/platformio.ini +++ b/platformio.ini @@ -57,7 +57,7 @@ lib_deps = https://github.com/tonton81/FlexCAN_T4 https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/86/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/90/can_lib.tar.gz [env:test_can_on_teensy] lib_ignore = diff --git a/src/main.cpp b/src/main.cpp index ba491ada3..01e7abace 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,6 +23,8 @@ #include "SABInterface.h" #include "VectornavInterface.h" #include "LoadCellInterface.h" +#include "TorqueControllers.h" + /* Systems */ #include "SysClock.h" @@ -30,6 +32,7 @@ #include "SafetySystem.h" #include "DrivetrainSystem.h" #include "PedalsSystem.h" +// #include "TorqueControllerMux.h" #include "TorqueControllerMux.h" #include "CASESystem.h" @@ -141,6 +144,8 @@ struct inverters // SYSTEMS // */ + + SysClock sys_clock; SteeringSystem steering_system(&steering1); BuzzerController buzzer(BUZZER_ON_INTERVAL); @@ -150,7 +155,7 @@ SafetySystem safety_system(&ams_interface, &wd_interface); PedalsSystem pedals_system(accel_params, brake_params); using DriveSys_t = DrivetrainSystem; DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); -TorqueControllerMux torque_controller_mux(1.0, 0.4); +TorqueControllerMux<> torque_controller_mux({},{}); // TODO ensure that case uses max regen torque, right now its not CASEConfiguration case_config = { // Following used for generated code @@ -211,6 +216,8 @@ CASEConfiguration case_config = { CASESystem case_system(&CAN3_txBuffer, 100, 70, 550, case_config); +//// Controllers +TorqueControllerCASEWrapper CASE_wrapper(case_system); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); @@ -323,6 +330,16 @@ void loop() tick_all_interfaces(curr_tick); // tick systems + + // single source of truth for the state of the car. + // no systems or interfaces should write directly to this. + car_state car_state_inst{curr_tick, + load_cell_interface.getLoadCellForces(), + vn_interface.get_vn_struct(), + steering_system.getSteeringSystemData(), + drivetrain.get_dynamic_data(), + pedals_system.getPedalsSystemData()}; + tick_all_systems(curr_tick); // inverter procedure before entering state machine @@ -332,7 +349,8 @@ void loop() drivetrain.reset_drivetrain(); } // tick state machine - fsm.tick_state_machine(curr_tick.millis); + + fsm.tick_state_machine(curr_tick.millis, car_state_inst); // give the state of the car to the param interface param_interface.update_car_state(fsm.get_state()); diff --git a/test/test_systems/fake_controller_type.h b/test/test_systems/fake_controller_type.h new file mode 100644 index 000000000..efb7873fa --- /dev/null +++ b/test/test_systems/fake_controller_type.h @@ -0,0 +1,13 @@ +#ifndef __FAKE_CONTROLLER_TYPE_H__ +#define __FAKE_CONTROLLER_TYPE_H__ +#include "BaseController.h" + +class TestControllerType : public virtual Controller +{ + +public: + TorqueControllerOutput_s output; + TorqueControllerOutput_s evaluate(const car_state &state) override { return output; } + // void update_input_state(const car_state &state) override { } +}; +#endif // __FAKE_CONTROLLER_TYPE_H__ \ No newline at end of file diff --git a/test/test_systems/state_machine_test.h b/test/test_systems/state_machine_test.h index 74f312948..7e63b6369 100644 --- a/test/test_systems/state_machine_test.h +++ b/test/test_systems/state_machine_test.h @@ -3,7 +3,15 @@ #include #include #include "MCUStateMachine.h" +#include "fake_controller_type.h" +class FakeTCMux +{ +public: + DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e requested_torque_limit, + const car_state &input_state) { return {}; } +}; class DrivetrainMock { @@ -24,21 +32,23 @@ class DrivetrainMock bool drivetrain_error_occured() { return drivetrain_error_; }; void command_drivetrain(const DrivetrainCommand_s &data){}; - void disable_no_pins() {}; + void disable_no_pins(){}; }; -void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) +car_state dummy_state({}, {}, {}, {}, {}, {}); + +void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) { // ticking without hv over threshold testing and ensuring the tractive system not active still auto sys_time2 = sys_time; drivetrain.hv_thresh_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); // hv going over threshold -> tractive system active drivetrain.hv_thresh_ = true; sys_time2 += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time2 += 1; dash_interface.start_button_status_ = true; AnalogConversion_s pedals1_data; @@ -54,56 +64,57 @@ void handle_startup(MCUStateMachine &state_machine, unsigned lon auto pedals4_data = pedals3_data; pedals.tick(SysTick_s{}, pedals1_data, pedals2_data, pedals3_data, pedals4_data); // get to enabling inverters - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); } TEST(MCUStateMachineTesting, test_state_machine_init_tick) { - - AMSInterface ams(0,0,0,0,0); + + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; EXPECT_EQ(state_machine.get_state(), CAR_STATE::STARTUP); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) { - AMSInterface ams(0,0,0,0,0); + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; // ticking without hv over threshold testing and ensuring the tractive system not active still - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); // hv going over threshold -> tractive system drivetrain.hv_thresh_ = true; sys_time += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); // hv going under thresh -> tractive system not active drivetrain.hv_thresh_ = false; sys_time += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } @@ -111,28 +122,28 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) { unsigned long sys_time = 1000; - AMSInterface ams(0,0,0,0,0); + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; - + + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); // ticking without hv over threshold testing and ensuring the tractive system not active still - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); // hv going over threshold -> tractive system drivetrain.hv_thresh_ = true; sys_time += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); sys_time += 1; @@ -150,22 +161,22 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) pedals3_data.status = pedals1_data.status; auto pedals4_data = pedals3_data; pedals.tick(SysTick_s{}, pedals1_data, pedals2_data, pedals3_data, pedals4_data); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); } // test getting into and out of the waiting RTD and ensuring it stays within the state when we want it to TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) { - AMSInterface ams(0,0,0,0,0); + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); @@ -173,33 +184,33 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) drivetrain.hv_thresh_ = true; drivetrain.drivetrain_inited_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 20; dash_interface.buzzer = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 35; dash_interface.buzzer = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); } TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) { - AMSInterface ams(0,0,0,0,0); + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; @@ -208,14 +219,14 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); drivetrain.drivetrain_inited_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); buzzer.activate_buzzer(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 20; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } @@ -224,18 +235,16 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_active) { - AMSInterface ams(0,0,0,0,0); + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; drivetrain.drivetrain_error_ = false; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); - - + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; @@ -245,25 +254,25 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti sys_time += 70; drivetrain.drivetrain_inited_ = true; drivetrain.hv_thresh_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 70; drivetrain.hv_thresh_ = true; dash_interface.buzzer = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); dash_interface.buzzer = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); drivetrain.drivetrain_error_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); } @@ -271,20 +280,18 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_active) { - AMSInterface ams(0,0,0,0,0); + AMSInterface ams(0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; drivetrain.drivetrain_error_ = false; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + FakeTCMux tc_mux; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); - - - + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); @@ -293,26 +300,26 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ sys_time += 70; drivetrain.drivetrain_inited_ = true; drivetrain.hv_thresh_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 70; drivetrain.hv_thresh_ = true; dash_interface.buzzer = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); dash_interface.buzzer = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); // drivetrain.drivetrain_error_ = true; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index e8e5ab090..0ca9d8295 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -1,17 +1,26 @@ #ifndef __TC_MUX_V2_TESTING_H__ #define __TC_MUX_V2_TESTING_H__ -#include "TorqueControllerMuxV2.h" +#include "TorqueControllerMux.h" +#include "TorqueControllers.h" +#include "fake_controller_type.h" +struct dummy_queue +{ +}; + +void assert_good_output() template -void set_four_outputs(quad_array_type& out, float val) +void set_four_outputs(quad_array_type &out, float val) { out[0] = val; out[1] = val; out[2] = val; out[3] = val; } + + template void set_outputs(controller_type &controller, float mps, float torque) { @@ -25,19 +34,12 @@ void set_outputs(controller_type &controller, float mps, float torque) controller.output.command.torqueSetpoints[3] = torque; } -class TestControllerType : public virtual Controller -{ -public: - TorqueControllerOutput_s output; - TorqueControllerOutput_s evaluate(const car_state &state) override { return output; } - // void update_input_state(const car_state &state) override { } -}; TEST(TorqueControllerMuxTesting, test_construction) { TestControllerType inst1, inst2; - TorqueControllerMuxv2<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); } // TEST(TorqueControllerMuxTesting, test_) @@ -49,8 +51,8 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) TestControllerType inst1, inst2; set_outputs(inst1, 0, 1); set_outputs(inst2, 6, 1); - TorqueControllerMuxv2<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - car_state state; + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + car_state state({}, {}, {}, {}, {}, {}); set_four_outputs(state.drivetrain_data.measuredSpeeds, 10000.0); state.pedals_data = {}; @@ -58,7 +60,6 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - // 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); @@ -75,5 +76,54 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); } -// TEST(TorqueControllerMuxTesting) +TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) +{ + // mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + dummy_queue q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + + TorqueControllerMux<5> + torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, true, false, false, false}); +} + +TEST(TorqueControllerMuxTesting, test_evaluation) +{ + // mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + dummy_queue q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + + TorqueControllerMux<5> torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, true, false, false, false}); + + +} #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file diff --git a/test/test_systems/test_CASE.h b/test/test_systems/test_CASE.h index 50fb5c448..bbfa35563 100644 --- a/test/test_systems/test_CASE.h +++ b/test/test_systems/test_CASE.h @@ -20,7 +20,7 @@ TEST(CASESystemTesting, test_vn_start_time) CASEConfiguration case_config = { // Following used for generated code - .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m, Torque limit used for yaw pid torque split overflow + .AbsoluteTorqueLimit = PhysicalParameters::AMK_MAX_TORQUE, // N-m, Torque limit used for yaw pid torque split overflow .yaw_pid_p = 1.369, .yaw_pid_i = 0.25, .yaw_pid_d = 0.0, @@ -53,7 +53,7 @@ TEST(CASESystemTesting, test_vn_start_time) .yaw_pid_brakes_d = 0.0, .decoupledYawPIDBrakesMaxDIfference = 2, // N-m .discontinuousBrakesPercentThreshold = 0.4, - .TorqueMode = AMK_MAX_TORQUE, // N-m + .TorqueMode = PhysicalParameters::AMK_MAX_TORQUE, // N-m .RegenLimit = -10.0, // N-m .useNoRegen5kph = true, .useTorqueBias = true, @@ -67,9 +67,9 @@ TEST(CASESystemTesting, test_vn_start_time) .tcs_pid_upper_rpm_rear = 5000.0, // RPM // Following used for calculate_torque_request in CASESystem.tpp - .max_rpm = AMK_MAX_RPM, - .max_regen_torque = AMK_MAX_TORQUE, - .max_torque = AMK_MAX_TORQUE, + .max_rpm = PhysicalParameters::AMK_MAX_RPM, + .max_regen_torque = PhysicalParameters::AMK_MAX_TORQUE, + .max_torque = PhysicalParameters::AMK_MAX_TORQUE, }; CASESystem case_system(&faked_msg_q, 100, 70, 550, case_config); } From 861ea02ffcc308638831c6e45d36a222c76e49eb Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 11 May 2024 14:37:54 -0400 Subject: [PATCH 05/31] adding in more tests for tc mux --- lib/systems/include/SimpleController.h | 2 +- lib/systems/include/TorqueControllerMux.tpp | 4 + lib/systems/src/SimpleController.cpp | 78 +++++++++---------- .../launch_controller_integration_testing.h | 1 + test/test_systems/tc_mux_v2_testing.h | 74 +++++++++++++++--- 5 files changed, 103 insertions(+), 56 deletions(-) create mode 100644 test/test_systems/launch_controller_integration_testing.h diff --git a/lib/systems/include/SimpleController.h b/lib/systems/include/SimpleController.h index 46f12db27..2892ec490 100644 --- a/lib/systems/include/SimpleController.h +++ b/lib/systems/include/SimpleController.h @@ -29,7 +29,7 @@ class TorqueControllerSimple : public Controller writeout_.ready = true; } - void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData); + void tick(const PedalsSystemData_s &pedalsData); TorqueControllerOutput_s evaluate(const car_state &state) override; }; diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index 7b4cdd2da..361a53127 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -9,6 +9,7 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C current_output = controller_pointers_[static_cast(current_status_.current_controller_mode_)]->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_; if (requesting_controller_change) @@ -26,10 +27,13 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C if (!mux_bypass_limits_[static_cast(current_status_.current_controller_mode_)]) { current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); + // std::cout << "output torques after regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_torque_limit_(current_output.command, 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); } + + // 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; return current_output.command; } diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/SimpleController.cpp index 865d51555..3ffc42549 100644 --- a/lib/systems/src/SimpleController.cpp +++ b/lib/systems/src/SimpleController.cpp @@ -1,55 +1,47 @@ #include "SimpleController.h" -void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData) +void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) { - // Calculate torque commands at 100hz - if (tick.triggers.trigger100) + // Both pedals are not pressed and no implausibility has been detected + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + torqueRequest = accelRequest * PhysicalParameters::AMK_MAX_TORQUE; + + writeout_.command.speeds_rpm[FL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = PhysicalParameters::AMK_MAX_RPM; + 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_; + } + else { - // Both pedals are not pressed and no implausibility has been detected - // accelRequest goes between 1.0 and -1.0 - float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; - float torqueRequest; - - if (accelRequest >= 0.0) - { - // Positive torque request - torqueRequest = accelRequest * PhysicalParameters::AMK_MAX_TORQUE; - - // writeout_.command.speeds_rpm[FL] = accelRequest * PhysicalParameters::AMK_MAX_RPM; - // writeout_.command.speeds_rpm[FR] = accelRequest * PhysicalParameters::AMK_MAX_RPM;pid_input_ - // writeout_.command.speeds_rpm[RL] = accelRequest * PhysicalParameters::AMK_MAX_RPM; - // writeout_.command.speeds_rpm[RR] = accelRequest * PhysicalParameters::AMK_MAX_RPM; - writeout_.command.speeds_rpm[FL] = PhysicalParameters::AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = PhysicalParameters::AMK_MAX_RPM; - 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_; - } - else - { - // Negative torque request - torqueRequest = PhysicalParameters::MAX_REGEN_TORQUE * accelRequest * -1.0; - - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - 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_; - } + // Negative torque request + torqueRequest = PhysicalParameters::MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + 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_; } } TorqueControllerOutput_s TorqueControllerSimple::evaluate(const car_state &state) { - tick(state.systick, state.pedals_data); + tick(state.pedals_data); return writeout_; } \ No newline at end of file diff --git a/test/test_systems/launch_controller_integration_testing.h b/test/test_systems/launch_controller_integration_testing.h new file mode 100644 index 000000000..c6541548e --- /dev/null +++ b/test/test_systems/launch_controller_integration_testing.h @@ -0,0 +1 @@ +// TODO put the launch controller testing in here with the tc mux diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 0ca9d8295..729071c52 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -5,11 +5,14 @@ #include "TorqueControllers.h" #include "fake_controller_type.h" +// TODO add back in test for torque limits +// TODO add back in the power limit testing + struct dummy_queue { }; -void assert_good_output() +// void assert_good_output() template void set_four_outputs(quad_array_type &out, float val) @@ -20,7 +23,6 @@ void set_four_outputs(quad_array_type &out, float val) out[3] = val; } - template void set_outputs(controller_type &controller, float mps, float torque) { @@ -34,8 +36,6 @@ void set_outputs(controller_type &controller, float mps, float torque) controller.output.command.torqueSetpoints[3] = torque; } - - TEST(TorqueControllerMuxTesting, test_construction) { TestControllerType inst1, inst2; @@ -65,7 +65,7 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) 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); - + set_outputs(inst1, 0, 1); set_outputs(inst2, 0, 1); set_four_outputs(state.drivetrain_data.measuredSpeeds, 0); @@ -76,6 +76,37 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); } +TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) +{ + TestControllerType inst1, inst2; + set_outputs(inst1, 0.1, 1); + set_outputs(inst2, 3, 10); + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + car_state state({}, {}, {}, {}, {}, {}); + + 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); + + // tick it a bunch of times + 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); + 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); + 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().current_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); +} + TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) { // mode 0 @@ -116,14 +147,33 @@ TEST(TorqueControllerMuxTesting, test_evaluation) TorqueControllerSimpleLaunch simple_launch; // mode 4 TorqueControllerSlipLaunch slip_launch; - + float max_torque = 21.42; TorqueControllerMux<5> torque_controller_mux({static_cast(&tc_simple), - static_cast(&tc_vec), - static_cast(&case_wrapper), - static_cast(&simple_launch), - static_cast(&slip_launch)}, - {false, true, false, false, false}); + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, true, false, false, false}); - + car_state 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); + + + 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); + + + // out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_1_input_state); } + + #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file From ff3f66d83251710f6ca1fcf215aef347659769dd Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 11 May 2024 20:34:20 -0400 Subject: [PATCH 06/31] redid launch controller integration tests --- lib/shared_data/include/PhysicalParameters.h | 10 +- lib/shared_data/include/SharedDataTypes.h | 3 +- lib/systems/include/TorqueControllerMux.h | 6 +- lib/systems/include/TorqueControllerMux.tpp | 38 +- lib/systems/src/BaseLaunchController.cpp | 6 +- test/test_systems/fake_controller_type.h | 3 + .../launch_controller_integration_testing.h | 183 +++++ test/test_systems/main.cpp | 1 + test/test_systems/tc_mux_v2_testing.h | 108 ++- .../test_systems/torque_controller_mux_test.h | 770 ------------------ 10 files changed, 323 insertions(+), 805 deletions(-) delete mode 100644 test/test_systems/torque_controller_mux_test.h diff --git a/lib/shared_data/include/PhysicalParameters.h b/lib/shared_data/include/PhysicalParameters.h index 916548537..c99f50bf7 100644 --- a/lib/shared_data/include/PhysicalParameters.h +++ b/lib/shared_data/include/PhysicalParameters.h @@ -8,11 +8,11 @@ namespace PhysicalParameters const float MAX_REGEN_TORQUE = 10.0; } -const float GEARBOX_RATIO = 11.86; -const float WHEEL_DIAMETER = 0.4064; // meters -const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; -const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0 / 1000.0; -const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; +constexpr const float GEARBOX_RATIO = 11.86; +constexpr const float WHEEL_DIAMETER = 0.4064; // meters +constexpr const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; +constexpr const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0 / 1000.0; +constexpr const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415 / 60.0; const float RAD_PER_SECOND_TO_RPM = 1 / RPM_TO_RAD_PER_SECOND; diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 33eea4298..44e3800a5 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -110,7 +110,8 @@ enum class TorqueControllerMuxError { NO_ERROR = 0, ERROR_SPEED_DIFF_TOO_HIGH = 1, - ERROR_TORQUE_DIFF_TOO_HIGH = 2 + ERROR_TORQUE_DIFF_TOO_HIGH = 2, + ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS =3 }; struct TorqueControllerMuxStatus diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index c27d2cd95..014d21660 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -45,9 +45,9 @@ namespace TC_MUX_DEFAULT_PARAMS { - const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s - const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm - const float MAX_POWER_LIMIT = 630000.0; + 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; }; diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index 361a53127..e4a1e3b1b 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -5,9 +5,23 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C TorqueLimit_e requested_torque_limit, const car_state &input_state) { - TorqueControllerOutput_s current_output; - current_output = controller_pointers_[static_cast(current_status_.current_controller_mode_)]->evaluate(input_state); + DrivetrainCommand_s empty_command = { + .speeds_rpm = {0, 0, 0, 0}, + .torqueSetpoints = {0, 0, 0, 0}}; + + TorqueControllerOutput_s current_output = { + .command = empty_command, + .ready = true}; + + int controller_mode_index = static_cast(current_status_.current_controller_mode_); + if (controller_mode_index > (controller_pointers_.size())) + { + current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; + return current_output.command; + } + + current_output = controller_pointers_[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_; @@ -24,12 +38,16 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C } current_status_.current_error = error_state; } - if (!mux_bypass_limits_[static_cast(current_status_.current_controller_mode_)]) + if (!mux_bypass_limits_[controller_mode_index]) { + + // std::cout << "output torques before regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); // std::cout << "output torques after regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_torque_limit_(current_output.command, 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]); + + // std::cout << "output torques after power limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_positive_speed_limit_(current_output.command); } @@ -70,7 +88,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_positive_speed_l out = command; for (int i = 0; i < NUM_MOTORS; i++) { - out.speeds_rpm[i] = std::max(0.0f, command.speeds_rpm[i]); + out.speeds_rpm[i] = std::max(0.0f, out.speeds_rpm[i]); } return out; } @@ -84,7 +102,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(co for (int i = 0; i < NUM_MOTORS; i++) { - avg_torque += abs(command.torqueSetpoints[i]); + avg_torque += abs(out.torqueSetpoints[i]); } avg_torque /= NUM_MOTORS; @@ -124,9 +142,11 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con net_power += abs(out.torqueSetpoints[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); } + // std::cout <<"net power "<< net_power < (power_limit)) { + // std::cout <<"net power too big" <::apply_power_limit_(con // 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 { maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain_data.measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); - allWheelsRegen &= (command.speeds_rpm[i] < abs(drivetrain_data.measuredSpeeds[i]) || command.speeds_rpm[i] == 0); + allWheelsRegen &= (out.speeds_rpm[i] < abs(drivetrain_data.measuredSpeeds[i]) || out.speeds_rpm[i] == 0); } // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH diff --git a/lib/systems/src/BaseLaunchController.cpp b/lib/systems/src/BaseLaunchController.cpp index 7871c9e4c..963acae2d 100644 --- a/lib/systems/src/BaseLaunchController.cpp +++ b/lib/systems/src/BaseLaunchController.cpp @@ -1,5 +1,5 @@ #include "BaseLaunchController.h" -#include /* abs */ +#include /* abs */ void BaseLaunchController::tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, @@ -98,15 +98,15 @@ void BaseLaunchController::tick( writeout_.command.torqueSetpoints[FR] = PhysicalParameters::AMK_MAX_TORQUE; writeout_.command.torqueSetpoints[RL] = PhysicalParameters::AMK_MAX_TORQUE; writeout_.command.torqueSetpoints[RR] = PhysicalParameters::AMK_MAX_TORQUE; + + break; } - break; default: break; } } } - TorqueControllerOutput_s BaseLaunchController::evaluate(const car_state &state) { tick(state.systick, state.pedals_data, state.drivetrain_data.measuredSpeeds, state.vn_data); diff --git a/test/test_systems/fake_controller_type.h b/test/test_systems/fake_controller_type.h index efb7873fa..93d5cc2b4 100644 --- a/test/test_systems/fake_controller_type.h +++ b/test/test_systems/fake_controller_type.h @@ -1,6 +1,9 @@ #ifndef __FAKE_CONTROLLER_TYPE_H__ #define __FAKE_CONTROLLER_TYPE_H__ #include "BaseController.h" +struct dummy_queue +{ +}; class TestControllerType : public virtual Controller { diff --git a/test/test_systems/launch_controller_integration_testing.h b/test/test_systems/launch_controller_integration_testing.h index c6541548e..dcf667450 100644 --- a/test/test_systems/launch_controller_integration_testing.h +++ b/test/test_systems/launch_controller_integration_testing.h @@ -1 +1,184 @@ // TODO put the launch controller testing in here with the tc mux + +#include "TorqueControllerMux.h" +#include "TorqueControllers.h" +#include "fake_controller_type.h" + +constexpr DrivetrainDynamicReport_s create_drive_report(float speed_fl, float speed_fr, float speed_rl, float speed_rr) +{ + DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { + .measuredInverterFLPackVoltage = 550, + .measuredSpeeds = { + speed_fl, + speed_fr, + speed_rl, + speed_rr}, + .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}}; + return simulated_slow_drivetrain_dynamics; +} + +constexpr PedalsSystemData_s create_pedals_data(float accel_percent, float brake_percent) +{ + PedalsSystemData_s pedals = { + .accelImplausible = false, + .brakeImplausible = false, + .brakePressed = false, + .brakeAndAccelPressedImplausibility = false, + .implausibilityExceededMaxDuration = false, + .accelPercent = accel_percent, + .brakePercent = brake_percent}; + return pedals; +} + +constexpr float slow_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM; +constexpr auto simulated_slow_drivetrain_dynamics = create_drive_report(slow_speed, slow_speed, slow_speed, slow_speed); +constexpr auto simulated_barely_launch_drivetrain_dynamics = create_drive_report(0, 2785.86, 0, 0); +constexpr auto simulated_no_launch_drivetrain_dynamics = create_drive_report(0, 2786.86, 0, 0); +constexpr float fast_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM; +constexpr auto simulated_fast_drivetrain_dynamics = create_drive_report(fast_speed, fast_speed, fast_speed, fast_speed); +constexpr auto simulated_full_accel_press = create_pedals_data(1.0, 0); +constexpr auto simulated_no_accel_press = create_pedals_data(0, 0); +constexpr auto simulated_accel_and_brake_press = create_pedals_data(1.0, 0.3); + +TEST(LaunchIntergationTesting, test_simple_launch_controller) +{ + + SysClock clock = SysClock(); + SysTick_s cur_tick; + cur_tick = clock.tick(0); + car_state still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}); + car_state pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + car_state no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + car_state barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + car_state one_sec_passed_in_launch_state(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + car_state one_sec_passed_in_launch_state_w_error(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_accel_and_brake_press, {}); + + // mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + dummy_queue q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + + TorqueControllerMux<5> + torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, false, true, false, false}); + + auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + // tick again to calculate state switch + + // tick again to calculate state switch + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY); + + // press accelerator but with one wheel right at max speed threshold + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, no_launch_allowed_state); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY); + + // // go back to launch ready state + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY); + + // press accel now with one wheelspeed barely under threshold + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, barely_launch_state); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state); + ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02); + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state_w_error); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY); +} + +TEST(LaunchIntergationTesting, test_slip_launch_controller) +{ + SysClock clock = SysClock(); + SysTick_s cur_tick; + cur_tick = clock.tick(0); + + vectornav vn_data{}; + // mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + dummy_queue q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + + TorqueControllerMux<5> + torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, false, true, false, false}); + + car_state still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}); + car_state pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + car_state no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + car_state barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + + auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + // getting to slip launch (mode 4) + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY); + + car_state one_hundredth_sec_passed_in_launch_state(clock.tick(10000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, one_hundredth_sec_passed_in_launch_state); + + car_state two_hundredth_sec_passed_in_launch_state(clock.tick(20000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, two_hundredth_sec_passed_in_launch_state); + + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); + + car_state more_accel_time_since_launch(clock.tick( BaseLaunchControllerParams::const_accel_time * 1000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, more_accel_time_since_launch); + + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::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 + car_state small_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, small_vn_vel); + printf("lower vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); + ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target + + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + // // 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 = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s + + car_state big_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 2000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, big_vn_vel); + + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + printf("launch vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); + ASSERT_EQ((int16_t)res.speeds_rpm[0] > (BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); + + // TODO this is actually failing, idk what is going on here @walker + // ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02); +} \ No newline at end of file diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 12876f0ac..fdbe60b74 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -3,6 +3,7 @@ #include "state_machine_test.h" #include "pedals_system_test.h" // #include "torque_controller_mux_test.h" +#include "launch_controller_integration_testing.h" #include "tc_mux_v2_testing.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 729071c52..9a7703cfe 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -5,15 +5,10 @@ #include "TorqueControllers.h" #include "fake_controller_type.h" -// TODO add back in test for torque limits -// TODO add back in the power limit testing - -struct dummy_queue -{ -}; -// void assert_good_output() +// TODO +// - [ ] test to ensure that the size checking for desired modes works and failes properly template void set_four_outputs(quad_array_type &out, float val) { @@ -35,7 +30,18 @@ void set_outputs(controller_type &controller, float mps, float torque) controller.output.command.torqueSetpoints[2] = torque; controller.output.command.torqueSetpoints[3] = torque; } - +template +void set_output_rpm(controller_type &controller, float rpm, float torque) +{ + controller.output.command.speeds_rpm[0] = rpm; + 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; +} TEST(TorqueControllerMuxTesting, test_construction) { TestControllerType inst1, inst2; @@ -65,7 +71,7 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) 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); - + set_outputs(inst1, 0, 1); set_outputs(inst2, 0, 1); set_four_outputs(state.drivetrain_data.measuredSpeeds, 0); @@ -98,7 +104,7 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) 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().current_controller_mode_, ControllerMode_e::MODE_0); ASSERT_EQ(out1.torqueSetpoints[0], 1); @@ -129,11 +135,13 @@ TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) static_cast(&case_wrapper), static_cast(&simple_launch), static_cast(&slip_launch)}, - {false, true, false, false, false}); + {false, false, true, false, false}); } -TEST(TorqueControllerMuxTesting, test_evaluation) +TEST(TorqueControllerMuxTesting, test_mode0_evaluation) { + + float max_torque = 21.42; // mode 0 TorqueControllerSimple tc_simple(1.0f, 1.0f); // mode 1 @@ -147,14 +155,12 @@ TEST(TorqueControllerMuxTesting, test_evaluation) TorqueControllerSimpleLaunch simple_launch; // mode 4 TorqueControllerSlipLaunch slip_launch; - float max_torque = 21.42; TorqueControllerMux<5> torque_controller_mux({static_cast(&tc_simple), static_cast(&tc_vec), static_cast(&case_wrapper), static_cast(&simple_launch), static_cast(&slip_launch)}, {false, true, false, false, false}); - car_state 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); @@ -163,7 +169,6 @@ TEST(TorqueControllerMuxTesting, test_evaluation) ASSERT_NEAR(out.torqueSetpoints[2], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[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); @@ -171,9 +176,80 @@ TEST(TorqueControllerMuxTesting, test_evaluation) ASSERT_EQ(out.torqueSetpoints[2], 0); ASSERT_EQ(out.torqueSetpoints[3], 0); - // out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_1_input_state); } +TEST(TorqueControllerMuxTesting, test_power_limit) +{ + TestControllerType inst1; + set_output_rpm(inst1, 20000, 10.0); + TorqueControllerMux<1> test({static_cast(&inst1)}, {false}); + + DrivetrainDynamicReport_s drivetrain_data = {}; + for (int i = 0; i < 4; i++) + { + drivetrain_data.measuredSpeeds[i] = 500.0f; + } + car_state mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + + DrivetrainCommand_s res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); + + for (int i = 0; i < 4; i++) + { + ASSERT_EQ(res.torqueSetpoints[i], 10.0f); + } + for (int i = 0; i < 4; i++) + { + drivetrain_data.measuredSpeeds[i] = 20000.0f; + } + set_output_rpm(inst1, 20000, 21.0); + + car_state mode_0_input_state_high_power({}, {}, drivetrain_data, {}, {.accelPercent = 1.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state_high_power); + + for (int i = 0; i < 4; i++) + { + ASSERT_LT(res.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator + } +} + +TEST(TorqueControllerMuxTesting, test_torque_limit) +{ + + TestControllerType inst1; + + set_output_rpm(inst1, 500, 10.0); + inst1.output.command.torqueSetpoints[0] = 5; + TorqueControllerMux<1> test({static_cast(&inst1)}, {false}); + DrivetrainDynamicReport_s drivetrain_data = {}; + for (int i = 0; i < 4; i++) + { + drivetrain_data.measuredSpeeds[i] = 500.0f; + } + + car_state mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + + 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); + + set_output_rpm(inst1, 500, 20.0); + inst1.output.command.torqueSetpoints[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); + + 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]); +} #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h deleted file mode 100644 index 0ab60785d..000000000 --- a/test/test_systems/torque_controller_mux_test.h +++ /dev/null @@ -1,770 +0,0 @@ -#ifndef TORQUE_CONTROLLER_MUX_TEST -#define TORQUE_CONTROLLER_MUX_TEST - -#include -#include -#include -#include -#include -#include - - -#include "AnalogSensorsInterface.h" -#include "DashboardInterface.h" -#include "PhysicalParameters.h" -#include "TorqueControllers.h" -#include "VectornavInterface.h" - -// TODO: Find out why this test intermittently fails -// TEST(TorqueControllerMuxTesting, test_torque_button) -// { -// const int SERIES_LENGTH = 10; -// SysClock clock = SysClock(); -// SysTick_s cur_tick; -// TorqueControllerMux torque_controller_mux = TorqueControllerMux(); - -// bool buttonTimeSeries[SERIES_LENGTH]; // 50hz time series -// // Cycle back to low test -// buttonTimeSeries[1] = true; -// buttonTimeSeries[3] = true; -// buttonTimeSeries[5] = true; -// // Cycle only on button release test -// for (int i = 7; i < SERIES_LENGTH; i++) -// buttonTimeSeries[i] = true; - -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); - -// // Run test -// for (int i = 0; i < SERIES_LENGTH; i++) -// { -// cur_tick = clock.tick(i * 1000 * 20); // 20 ms increments -// torque_controller_mux.tick( -// cur_tick, -// (const DrivetrainDynamicReport_s) {}, -// (const PedalsSystemData_s) {}, -// (const SteeringSystemData_s) {}, -// (const AnalogConversion_s) {}, -// (const AnalogConversion_s) {}, -// (const AnalogConversion_s) {}, -// (const AnalogConversion_s) {}, -// ControllerMode_e::MODE_1, -// buttonTimeSeries[i] -// ); -// // Test conditions -// if (i == 2) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_MID_TORQUE); -// if (i == 4) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_FULL_TORQUE); -// if (i == 6) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); -// if (i == SERIES_LENGTH - 1) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); -// } -// } - -TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) -{ - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); - DrivetrainCommand_s resulting_torque_command; - - vectornav vn_data{}; - - DrivetrainDynamicReport_s simulated_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0.0, 0.0, 0.0, 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} - }; - - 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 - }; - - // Press the pedal and observe whether torque mode changes - // Torque mode should not change since the requested torque is vastly different from the currently requested torque (0) - torque_controller_mux.tick( - cur_tick, - simulated_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_LE(resulting_torque_command.torqueSetpoints[i], 0.0001); - ASSERT_LE(resulting_torque_command.speeds_rpm[i], 0.0001); - } - - // Release the pedal. The mode should change now - torque_controller_mux.tick( - cur_tick, - simulated_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // Now press the pedal again. Torque should be requested - torque_controller_mux.tick( - cur_tick, - simulated_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_GT(resulting_torque_command.torqueSetpoints[i], 5.0); - ASSERT_GT(resulting_torque_command.speeds_rpm[i], 5.0); - } -} - -TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) -{ - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); - DrivetrainCommand_s resulting_torque_command; - vectornav 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_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 - }; - - // Pretend the drivetrain is moving above MAX_SPEED_FOR_MODE_CHANGE and attempt mode shift (TC_NO_CONTROLLER to TC_SAFE_MODE) via tick - // Torque mode should not change since the car is moving - torque_controller_mux.tick( - cur_tick, - simulated_fast_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_LE(resulting_torque_command.torqueSetpoints[i], 0.0001); - ASSERT_LE(resulting_torque_command.speeds_rpm[i], 0.0001); - } - - // Tell TCMUX vehicle is stationary. Mode should change - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // Now press the pedal. Torque should be requested - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_GT(resulting_torque_command.torqueSetpoints[i], 5.0); - ASSERT_GT(resulting_torque_command.speeds_rpm[i], 5.0); - } -} - -TEST(TorqueControllerMuxTesting, test_power_limit) { - TorqueControllerMux mux = TorqueControllerMux(); - DrivetrainCommand_s drive_command; - - DrivetrainDynamicReport_s edit; - - for (int i = 0; i < 4; i++) { - edit.measuredSpeeds[i] = 500.0f; - drive_command.torqueSetpoints[i] = 10.0f; - } - - const DrivetrainDynamicReport_s drivetrain1 = edit; - - mux.applyPowerLimit(&drive_command, &drivetrain1); - - for (int i = 0; i < 4; i++) { - ASSERT_EQ(drive_command.torqueSetpoints[i], 10.0f); - } - - for (int i = 0; i < 4; i++) { - edit.measuredSpeeds[i] = 20000.0f; - drive_command.torqueSetpoints[i] = 21.0f; - } - - const DrivetrainDynamicReport_s drivetrain2 = edit; - - mux.applyPowerLimit(&drive_command, &drivetrain2); - - for (int i = 0; i < 4; i++) { - ASSERT_LT(drive_command.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator - } - -} - -TEST(TorqueControllerMuxTesting, test_torque_limit) { - TorqueControllerMux mux = TorqueControllerMux(); - DrivetrainCommand_s drive_command; - - for (int i = 0; i < 4; i++) { - drive_command.speeds_rpm[i] = 500.0f; - drive_command.torqueSetpoints[i] = 10.0f; - } - - drive_command.torqueSetpoints[0] = 5; - - mux.applyTorqueLimit(&drive_command); - - 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); - - for (int i = 0; i < 4; i++) { - drive_command.speeds_rpm[i] = 500.0f; - drive_command.torqueSetpoints[i] = 20.0f; - } - drive_command.torqueSetpoints[0] = 5; - - mux.applyTorqueLimit(&drive_command); - - 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); - - 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]); -} - -TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); - DrivetrainCommand_s resulting_torque_command; - - vectornav 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) {}, - ControllerMode_e::MODE_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) {}, - ControllerMode_e::MODE_3, - 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) {}, - ControllerMode_e::MODE_3, - 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 accelerator but with one wheel right at max speed threshold - torque_controller_mux.tick( - cur_tick, - simulated_no_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_3, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_NOT_READY); - - // go back to launch ready state - torque_controller_mux.tick( - cur_tick, - simulated_barely_launch_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_3, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - 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( - cur_tick, - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_3, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - torque_controller_mux.tick( - clock.tick(1000000), // 1 second since launch - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_3, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); - - // 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); - - torque_controller_mux.tick( - clock.tick(1000000), // 1 second since launch - simulated_barely_launch_drivetrain_dynamics, - simulated_accel_and_brake_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - ControllerMode_e::MODE_3, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - 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); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); - DrivetrainCommand_s resulting_torque_command; - - vectornav 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) {}, - ControllerMode_e::MODE_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) {}, - ControllerMode_e::MODE_4, - 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) {}, - ControllerMode_e::MODE_4, - 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) {}, - ControllerMode_e::MODE_4, - 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) {}, - ControllerMode_e::MODE_4, - 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) {}, - ControllerMode_e::MODE_4, - 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) {}, - ControllerMode_e::MODE_4, - 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) {}, - ControllerMode_e::MODE_4, - 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 From 1bf61c1769285b8c43fbd27e7b992b1ff067485c Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 11 May 2024 20:51:30 -0400 Subject: [PATCH 07/31] finishing the unit testing of the new tc mux --- lib/systems/include/TorqueControllerMux.tpp | 19 ++++++++++--------- test/test_systems/main.cpp | 1 - test/test_systems/tc_mux_v2_testing.h | 19 +++++++++++++++++-- 3 files changed, 27 insertions(+), 12 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index e4a1e3b1b..c6242a010 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -6,39 +6,40 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C const car_state &input_state) { - DrivetrainCommand_s empty_command = { - .speeds_rpm = {0, 0, 0, 0}, - .torqueSetpoints = {0, 0, 0, 0}}; + DrivetrainCommand_s empty_command = BaseControllerParams::TC_COMMAND_NO_TORQUE; TorqueControllerOutput_s current_output = { .command = empty_command, .ready = true}; - int controller_mode_index = static_cast(current_status_.current_controller_mode_); - if (controller_mode_index > (controller_pointers_.size())) + int req_controller_mode_index = static_cast(requested_controller_type); + int current_controller_mode_index = static_cast(current_status_.current_controller_mode_); + + if (req_controller_mode_index > ( controller_pointers_.size() - 1 )) { current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; - return current_output.command; + return empty_command; } - current_output = controller_pointers_[controller_mode_index]->evaluate(input_state); + current_output = controller_pointers_[current_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_; if (requesting_controller_change) { - TorqueControllerOutput_s proposed_output = controller_pointers_[static_cast(requested_controller_type)]->evaluate(input_state); + TorqueControllerOutput_s proposed_output = controller_pointers_[req_controller_mode_index]->evaluate(input_state); TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); // std::cout << "error state " << static_cast(error_state) << std::endl; if (error_state == TorqueControllerMuxError::NO_ERROR) { current_status_.current_controller_mode_ = requested_controller_type; + current_controller_mode_index = req_controller_mode_index; current_output = proposed_output; } current_status_.current_error = error_state; } - if (!mux_bypass_limits_[controller_mode_index]) + if (!mux_bypass_limits_[current_controller_mode_index]) { // std::cout << "output torques before regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index fdbe60b74..c5160a072 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -2,7 +2,6 @@ #include "state_machine_test.h" #include "pedals_system_test.h" -// #include "torque_controller_mux_test.h" #include "launch_controller_integration_testing.h" #include "tc_mux_v2_testing.h" #include "drivetrain_system_test.h" diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 9a7703cfe..97fa896c7 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -8,7 +8,7 @@ // TODO -// - [ ] test to ensure that the size checking for desired modes works and failes properly +// - [x] test to ensure that the size checking for desired modes works and failes properly template void set_four_outputs(quad_array_type &out, float val) { @@ -48,7 +48,22 @@ TEST(TorqueControllerMuxTesting, test_construction) TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); } -// TEST(TorqueControllerMuxTesting, test_) +TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) +{ + TestControllerType inst1, inst2; + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + car_state 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); + for (int i =0; i< 4; i++) + { + + ASSERT_EQ(res.speeds_rpm[i], 0.0); + ASSERT_EQ(res.torqueSetpoints[i], 0.0); + } +} + // ensure that swapping to a controller that has a higher desired output speed than previously // commanded that we dont switch From 4a2d442fb52c823d3f8dfc8068f696031a80fd24 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 11 May 2024 21:48:29 -0400 Subject: [PATCH 08/31] finished integration and ready for on car testing --- lib/interfaces/include/DashboardInterface.h | 4 +- lib/interfaces/include/TelemetryInterface.h | 3 - lib/interfaces/src/DashboardInterface.cpp | 2 +- lib/interfaces/src/TelemetryInterface.cpp | 17 ---- lib/shared_data/include/SharedDataTypes.h | 30 +------ lib/systems/include/TorqueControllerMux.h | 2 +- lib/systems/include/TorqueControllerMux.tpp | 10 ++- platformio.ini | 5 +- src/main.cpp | 87 ++++++++++++--------- 9 files changed, 68 insertions(+), 92 deletions(-) diff --git a/lib/interfaces/include/DashboardInterface.h b/lib/interfaces/include/DashboardInterface.h index 556cae215..b5a4e33d9 100644 --- a/lib/interfaces/include/DashboardInterface.h +++ b/lib/interfaces/include/DashboardInterface.h @@ -87,7 +87,7 @@ class DashboardInterface /* The instantiated data struct used to access data by member functions */ DashComponentInterface_s _data; bool prev_button_pressed_state_; - update_torque_mode_(bool button_pressed); + void update_torque_mode_(bool button_pressed); public: /*! Constructor for new DashboardInterface, All that it is inited with @@ -97,7 +97,7 @@ class DashboardInterface */ DashboardInterface(CANBufferType *msg_output_queue) { - torque_limit_mode_ = TorqueLimit_e::TCMUX_FULL_TORQUE; + _data.torque_limit_mode = TorqueLimit_e::TCMUX_FULL_TORQUE; prev_button_pressed_state_ = false; msg_queue_ = msg_output_queue; }; diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index cf187839f..57a8dec85 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -111,8 +111,6 @@ class TelemetryInterface const AnalogConversion_s ¤t, const AnalogConversion_s &reference); - void enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data); - /* Enqueue outbound telemetry CAN messages */ // void enqueue_CAN_mcu_pedal_readings(); // void enqueue_CAN_mcu_load_cells(); @@ -147,7 +145,6 @@ class TelemetryInterface const AnalogConversion_s &brake_1, const AnalogConversion_s &brake_2, float mech_brake_active_percent, - const PIDTVTorqueControllerData &data, const TorqueControllerMuxError& current_mux_status); }; diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index a5ca43b4a..22210a81e 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -33,7 +33,7 @@ void DashboardInterface::update_torque_mode_(bool button_pressed) if (prev_button_pressed_state_ == true && button_pressed == false) { - _data.torque_limit_mode_ = static_cast((static_cast(_data.torque_limit_mode_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); + _data.torque_limit_mode = static_cast((static_cast(_data.torque_limit_mode) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); } prev_button_pressed_state_ = button_pressed; } diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 09e97089a..b11333fc7 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -223,21 +223,6 @@ void TelemetryInterface::enqueue_new_CAN(U *structure, uint32_t (*pack_function) msg_queue_->push_back(buf, sizeof(CAN_message_t)); } -void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data) -{ - CONTROLLER_PID_TV_DATA_t msg; - msg.controller_input_ro = HYTECH_controller_input_ro_toS(data.controller_input); - msg.controller_output_ro = HYTECH_controller_output_ro_toS(data.controller_output); - enqueue_new_CAN(&msg, &Pack_CONTROLLER_PID_TV_DATA_hytech); - - CONTROLLER_PID_TV_DELTA_DATA_t delta_msg; - delta_msg.pid_tv_fl_delta_ro = HYTECH_pid_tv_fl_delta_ro_toS(data.fl_torque_delta); - delta_msg.pid_tv_fr_delta_ro = HYTECH_pid_tv_fr_delta_ro_toS(data.fr_torque_delta); - delta_msg.pid_tv_rl_delta_ro = HYTECH_pid_tv_rl_delta_ro_toS(data.rl_torque_delta); - delta_msg.pid_tv_rr_delta_ro = HYTECH_pid_tv_rr_delta_ro_toS(data.rr_torque_delta); - - enqueue_new_CAN(&delta_msg, &Pack_CONTROLLER_PID_TV_DELTA_DATA_hytech); -} /* Tick SysClock */ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, @@ -258,7 +243,6 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversion_s &brake_1, const AnalogConversion_s &brake_2, float mech_brake_active_percent, - const PIDTVTorqueControllerData &data, const TorqueControllerMuxError ¤t_mux_status) { @@ -295,7 +279,6 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel], adc1.conversions[channels_.current_ref_channel]); - enqeue_controller_CAN_msg(data); update_front_thermistors_CAN_msg(mcu_adc.conversions[channels_.therm_fl_channel], mcu_adc.conversions[channels_.therm_fr_channel]); } \ No newline at end of file diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 44e3800a5..fce9b650b 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -3,34 +3,8 @@ #include #include "Utility.h" #include "SysClock.h" -enum class AnalogSensorStatus_e -{ - ANALOG_SENSOR_GOOD = 0, - ANALOG_SENSOR_CLAMPED = 1, -}; - -struct AnalogConversion_s -{ - int raw; - float conversion; - AnalogSensorStatus_e status; -}; +#include "SharedFirmwareTypes.h" -template -struct AnalogConversionPacket_s -{ - AnalogConversion_s conversions[N]; -}; - -struct PIDTVTorqueControllerData -{ - float controller_input; - float controller_output; - float fl_torque_delta; - float fr_torque_delta; - float rl_torque_delta; - float rr_torque_delta; -}; enum class TorqueLimit_e { TCMUX_FULL_TORQUE = 0, @@ -118,6 +92,8 @@ struct TorqueControllerMuxStatus { TorqueControllerMuxError current_error; ControllerMode_e current_controller_mode_; + TorqueLimit_e current_torque_limit_enum; + float current_torque_limit_value; bool output_is_bypassing_limits; }; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 014d21660..b5787a25a 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -107,7 +107,7 @@ class TorqueControllerMux static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); } - + const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } /// @brief function that evaluates the mux, controllers and gets the current command diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index c6242a010..4d391b76f 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -15,7 +15,7 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C int req_controller_mode_index = static_cast(requested_controller_type); int current_controller_mode_index = static_cast(current_status_.current_controller_mode_); - if (req_controller_mode_index > ( controller_pointers_.size() - 1 )) + if ((std::size_t)req_controller_mode_index > ( controller_pointers_.size() - 1 )) { current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; return empty_command; @@ -41,16 +41,20 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C } if (!mux_bypass_limits_[current_controller_mode_index]) { - + current_status_.current_torque_limit_enum = requested_torque_limit; // std::cout << "output torques before regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); // std::cout << "output torques after regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; 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]; current_output.command = apply_power_limit_(current_output.command, input_state.drivetrain_data, max_power_limit_, torque_limit_map_[requested_torque_limit]); - // std::cout << "output torques after power limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_positive_speed_limit_(current_output.command); } + else{ + current_status_.current_torque_limit_enum = TorqueLimit_e::TCMUX_FULL_TORQUE; + current_status_.current_torque_limit_value= PhysicalParameters::AMK_MAX_TORQUE; + } // 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; return current_output.command; diff --git a/platformio.ini b/platformio.ini index f981fa427..a2defde42 100644 --- a/platformio.ini +++ b/platformio.ini @@ -2,8 +2,9 @@ lib_deps_shared = Nanopb git+ssh://git@github.com/hytech-racing/CASE_lib.git#v45 - https://github.com/hytech-racing/shared_firmware_systems.git https://github.com/hytech-racing/HT_params/releases/download/2024-05-05T13_33_07/ht_eth_pb_lib.tar.gz + https://github.com/hytech-racing/shared_firmware_types.git#feb3b83 + https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 [env:test_env] platform = native @@ -53,7 +54,7 @@ extra_scripts = pre:extra_script.py lib_deps = ${common.lib_deps_shared} SPI - https://github.com/hytech-racing/shared_firmware_interfaces.git + https://github.com/hytech-racing/shared_firmware_interfaces.git#d4842588 https://github.com/tonton81/FlexCAN_T4 https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/RCMast3r/hytech_can#testing_new_inv_ids diff --git a/src/main.cpp b/src/main.cpp index 01e7abace..7154a04c6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -25,7 +25,6 @@ #include "LoadCellInterface.h" #include "TorqueControllers.h" - /* Systems */ #include "SysClock.h" #include "Buzzer.h" @@ -144,8 +143,6 @@ struct inverters // SYSTEMS // */ - - SysClock sys_clock; SteeringSystem steering_system(&steering1); BuzzerController buzzer(BUZZER_ON_INTERVAL); @@ -155,7 +152,7 @@ SafetySystem safety_system(&ams_interface, &wd_interface); PedalsSystem pedals_system(accel_params, brake_params); using DriveSys_t = DrivetrainSystem; DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); -TorqueControllerMux<> torque_controller_mux({},{}); + // TODO ensure that case uses max regen torque, right now its not CASEConfiguration case_config = { // Following used for generated code @@ -214,12 +211,30 @@ CASEConfiguration case_config = { .max_torque = 21.42, }; +//// Controllers CASESystem case_system(&CAN3_txBuffer, 100, 70, 550, case_config); +// mode 0 +TorqueControllerSimple tc_simple(1.0f, 1.0f); +// mode 1 +TorqueControllerLoadCellVectoring tc_vec; +// mode 2 +TorqueControllerCASEWrapper case_wrapper(&case_system); + +// mode 3 +TorqueControllerSimpleLaunch simple_launch; +// mode 4 +TorqueControllerSlipLaunch slip_launch; +const int number_of_controllers = 5; +TorqueControllerMux + torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, false, true, false, false}); -//// Controllers -TorqueControllerCASEWrapper CASE_wrapper(case_system); /* Declare state machine */ -MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); +MCUStateMachine> fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); // /* // GROUPING STRUCTS (To limit parameter count in utilizing functions) @@ -333,12 +348,12 @@ void loop() // single source of truth for the state of the car. // no systems or interfaces should write directly to this. - car_state car_state_inst{curr_tick, - load_cell_interface.getLoadCellForces(), - vn_interface.get_vn_struct(), + car_state car_state_inst(curr_tick, steering_system.getSteeringSystemData(), drivetrain.get_dynamic_data(), - pedals_system.getPedalsSystemData()}; + load_cell_interface.getLoadCellForces(), + pedals_system.getPedalsSystemData(), + vn_interface.get_vn_struct()); tick_all_systems(curr_tick); @@ -398,24 +413,35 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) TriggerBits_s t = current_system_tick.triggers; if (t.trigger10) // 10Hz { - dashboard.tick10( - &main_ecu, - int(fsm.get_state()), - buzzer.buzzer_is_on(), - drivetrain.drivetrain_error_occured(), - torque_controller_mux.getTorqueLimit(), - ams_interface.get_filtered_min_cell_voltage(), - telem_interface.get_glv_voltage(a1.get()), - static_cast(torque_controller_mux.activeController()->get_launch_state()), - dashboard.getDialMode()); + int slip_launch_state_int = static_cast(slip_launch.get_launch_state()); + int simple_launch_state_int = static_cast(simple_launch.get_launch_state()); + int combined_launch_state = 0; + if(slip_launch_state_int > 0){ + combined_launch_state = slip_launch_state_int; + } else if(simple_launch_state_int > 0) + { + combined_launch_state = simple_launch_state_int; + } else { + combined_launch_state = 0; + } + dashboard.tick10( + &main_ecu, + int(fsm.get_state()), + buzzer.buzzer_is_on(), + drivetrain.drivetrain_error_occured(), + torque_controller_mux.get_tc_mux_status().current_torque_limit_enum, + ams_interface.get_filtered_min_cell_voltage(), + telem_interface.get_glv_voltage(a1.get()), + combined_launch_state, + dashboard.getDialMode()); main_ecu.tick( static_cast(fsm.get_state()), drivetrain.drivetrain_error_occured(), safety_system.get_software_is_ok(), - static_cast(torque_controller_mux.getDriveMode()), - static_cast(torque_controller_mux.getTorqueLimit()), - torque_controller_mux.getMaxTorque(), + static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode_), + static_cast(torque_controller_mux.get_tc_mux_status().current_torque_limit_enum), + torque_controller_mux.get_tc_mux_status().current_torque_limit_value, buzzer.buzzer_is_on(), pedals_system.getPedalsSystemData(), ams_interface.pack_charge_is_critical(), @@ -442,7 +468,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); } if (t.trigger50) // 50Hz @@ -511,17 +537,6 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) 3); // case_system.update_config_from_param_interface(param_interface); - - torque_controller_mux.tick( - current_system_tick, - drivetrain.get_dynamic_data(), - pedals_system.getPedalsSystemData(), - steering_system.getSteeringSystemData(), - load_cell_interface.getLoadCellForces(), - dashboard.getDialMode(), - dashboard.torqueModeButtonPressed(), - vn_interface.get_vn_struct(), - controller_output); } void handle_ethernet_interface_comms() From 0d835767404c446848677c087bdfc1877fb35899 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 12 May 2024 19:45:35 -0400 Subject: [PATCH 09/31] resolving https://github.com/hytech-racing/MCU/pull/86#discussion_r1597670681 --- lib/interfaces/include/MCUInterface.h | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index 2c7f1d580..521f10301 100644 --- a/lib/interfaces/include/MCUInterface.h +++ b/lib/interfaces/include/MCUInterface.h @@ -7,7 +7,6 @@ #include "hytech.h" #include "MessageQueueDefine.h" #include "PedalsSystem.h" -#include "SharedDataTypes.h" const int DEFAULT_BMS_OK_READ = 17; // SHDN_D_READ const int DEFAULT_BMS_SENSE_PIN = 16; // BMS_OK_SENSE From e0c610b78c41350afe349a951cec80bfb3c0e64a Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 12 May 2024 19:49:15 -0400 Subject: [PATCH 10/31] resolving https://github.com/hytech-racing/MCU/pull/86#discussion_r1597672535 and https://github.com/hytech-racing/MCU/pull/86#discussion_r1597670681 --- lib/interfaces/include/VectornavInterface.h | 20 +------------------- 1 file changed, 1 insertion(+), 19 deletions(-) diff --git a/lib/interfaces/include/VectornavInterface.h b/lib/interfaces/include/VectornavInterface.h index 08a96ed53..13435859a 100644 --- a/lib/interfaces/include/VectornavInterface.h +++ b/lib/interfaces/include/VectornavInterface.h @@ -2,25 +2,7 @@ #define VECTORNAVINTERFACE #include "FlexCAN_T4.h" #include "hytech.h" - -// struct vectornav { -// float velocity_x; -// float velocity_y; -// float velocity_z; -// float linear_accel_x; -// float linear_accel_y; -// float linear_accel_z; -// float uncompLinear_accel[3]; // 3D uncompensated linear acceleration -// float yaw; -// float pitch; -// float roll; -// double latitude; -// double longitude; -// double ecef_coords[3]; // x,y,z -// uint64_t gps_time; // gps time -// uint8_t vn_status; // status -// xyz_vec angular_rates; -// }; +#include "SharedDataTypes.h" template class VNInterface From 8c0c3966db369803aafed7624ebba2bb4f3efbe7 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 12 May 2024 20:38:30 -0400 Subject: [PATCH 11/31] resolving https://github.com/hytech-racing/MCU/pull/86#discussion_r1597672943 --- lib/shared_data/include/SharedDataTypes.h | 29 +++++------------- lib/state_machine/include/MCUStateMachine.h | 2 +- lib/state_machine/include/MCUStateMachine.tpp | 2 +- lib/systems/include/BaseController.h | 2 +- lib/systems/include/BaseLaunchController.h | 2 +- lib/systems/include/CASEControllerWrapper.h | 2 +- .../include/LoadCellVectoringController.h | 2 +- lib/systems/include/SimpleController.h | 2 +- lib/systems/include/TorqueControllerMux.h | 2 +- lib/systems/include/TorqueControllerMux.tpp | 2 +- lib/systems/src/BaseLaunchController.cpp | 2 +- .../src/LoadCellVectoringController.cpp | 2 +- lib/systems/src/SimpleController.cpp | 2 +- src/main.cpp | 2 +- test/test_systems/fake_controller_type.h | 3 +- .../launch_controller_integration_testing.h | 30 +++++++++---------- test/test_systems/state_machine_test.h | 4 +-- test/test_systems/tc_mux_v2_testing.h | 14 ++++----- 18 files changed, 46 insertions(+), 60 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index fce9b650b..a40add941 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -119,25 +119,12 @@ struct SteeringSystemData_s SteeringSystemStatus_e status; }; -// struct TriggerBits_s -// { -// bool trigger1000 : 1; -// bool trigger500 : 1; -// bool trigger100 : 1; -// bool trigger50 : 1; -// bool trigger10 : 1; -// bool trigger5 : 1; -// bool trigger1 : 1; -// }; - -// struct SysTick_s -// { -// unsigned long millis; -// unsigned long micros; -// TriggerBits_s triggers; -// }; - -struct car_state +/// @brief car state struct that contains state of everything about the car including +// things such as steering, drivetrain, current system time, vectornav / INS data, +// etc. an instance of this struct is created in main and updated there by all of the systems +// and interfaces and then shared between all of the systems and interfaces that need +// access to the state of the car. +struct SharedCarState_s { // data SysTick_s systick; @@ -146,8 +133,8 @@ struct car_state LoadCellInterfaceOutput_s loadcell_data; PedalsSystemData_s pedals_data; vectornav vn_data; - car_state() = delete; - car_state(const SysTick_s &_systick, + SharedCarState_s() = delete; + SharedCarState_s(const SysTick_s &_systick, const SteeringSystemData_s &_steering_data, const DrivetrainDynamicReport_s &_drivetrain_data, const LoadCellInterfaceOutput_s &_loadcell_data, diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index e526fe39f..45ae41e74 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -48,7 +48,7 @@ class MCUStateMachine /// @brief function to tick the state machine. /// @param cm current millis from systick /// @param current_car_state current state of the car (not to be confused with the CAR_STATE enum which is the state machine state) - void tick_state_machine(unsigned long cm, const car_state ¤t_car_state); + void tick_state_machine(unsigned long cm, const SharedCarState_s ¤t_car_state); CAR_STATE get_state() { return current_state_; } bool car_in_ready_to_drive() { return current_state_ == CAR_STATE::READY_TO_DRIVE; }; diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index 1c36ab1b2..caddf9fad 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -1,7 +1,7 @@ #include "MCUStateMachine.h" template -void MCUStateMachine::tick_state_machine(unsigned long current_millis, const car_state ¤t_car_state) +void MCUStateMachine::tick_state_machine(unsigned long current_millis, const SharedCarState_s ¤t_car_state) { switch (get_state()) { diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index b6e76c8f4..5ae447050 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -15,7 +15,7 @@ class Controller float get_torque_request_(float torque_limit_nm, const PedalsSystemData_s &pedals_data) { return 0; }; public: - virtual TorqueControllerOutput_s evaluate(const car_state &state) = 0; + virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; }; #endif \ No newline at end of file diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 94ec879ed..68f8dcfe5 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -50,6 +50,6 @@ class BaseLaunchController : public virtual Controller const vectornav &vn_data); LaunchStates_e get_launch_state() { return launch_state_; } virtual void calc_launch_algo(const vectornav &vn_data) = 0; - TorqueControllerOutput_s evaluate(const car_state &state) override; + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; #endif // __BASELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/CASEControllerWrapper.h b/lib/systems/include/CASEControllerWrapper.h index a77b02d56..8e0dbd7e8 100644 --- a/lib/systems/include/CASEControllerWrapper.h +++ b/lib/systems/include/CASEControllerWrapper.h @@ -13,7 +13,7 @@ class TorqueControllerCASEWrapper : public virtual Controller TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) { } - TorqueControllerOutput_s evaluate(const car_state &state) override + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { DrivetrainCommand_s curr_cmd = case_instance_->get_current_drive_command(); TorqueControllerOutput_s out; diff --git a/lib/systems/include/LoadCellVectoringController.h b/lib/systems/include/LoadCellVectoringController.h index e371a8004..dc7aaa373 100644 --- a/lib/systems/include/LoadCellVectoringController.h +++ b/lib/systems/include/LoadCellVectoringController.h @@ -63,7 +63,7 @@ class TorqueControllerLoadCellVectoring : public virtual Controller const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const LoadCellInterfaceOutput_s &loadCellData); - TorqueControllerOutput_s evaluate(const car_state &state) override; + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; #endif \ No newline at end of file diff --git a/lib/systems/include/SimpleController.h b/lib/systems/include/SimpleController.h index 2892ec490..33f691fee 100644 --- a/lib/systems/include/SimpleController.h +++ b/lib/systems/include/SimpleController.h @@ -30,7 +30,7 @@ class TorqueControllerSimple : public Controller } void tick(const PedalsSystemData_s &pedalsData); - TorqueControllerOutput_s evaluate(const car_state &state) override; + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; #endif \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index b5787a25a..24152c02b 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -117,7 +117,7 @@ class TorqueControllerMux /// @return the current drivetrain command to be sent to the drivetrain DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e controller_command_torque_limit, - const car_state &input_state); + const SharedCarState_s &input_state); }; // } #include "TorqueControllerMux.tpp" diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index 4d391b76f..3253dbe28 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -3,7 +3,7 @@ template DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e requested_torque_limit, - const car_state &input_state) + const SharedCarState_s &input_state) { DrivetrainCommand_s empty_command = BaseControllerParams::TC_COMMAND_NO_TORQUE; diff --git a/lib/systems/src/BaseLaunchController.cpp b/lib/systems/src/BaseLaunchController.cpp index 963acae2d..cdbc6f6b2 100644 --- a/lib/systems/src/BaseLaunchController.cpp +++ b/lib/systems/src/BaseLaunchController.cpp @@ -107,7 +107,7 @@ void BaseLaunchController::tick( } } -TorqueControllerOutput_s BaseLaunchController::evaluate(const car_state &state) +TorqueControllerOutput_s BaseLaunchController::evaluate(const SharedCarState_s &state) { tick(state.systick, state.pedals_data, state.drivetrain_data.measuredSpeeds, state.vn_data); return writeout_; diff --git a/lib/systems/src/LoadCellVectoringController.cpp b/lib/systems/src/LoadCellVectoringController.cpp index e92241e92..f6d32c644 100644 --- a/lib/systems/src/LoadCellVectoringController.cpp +++ b/lib/systems/src/LoadCellVectoringController.cpp @@ -93,7 +93,7 @@ void TorqueControllerLoadCellVectoring::tick( } } -TorqueControllerOutput_s TorqueControllerLoadCellVectoring::evaluate(const car_state &state) +TorqueControllerOutput_s TorqueControllerLoadCellVectoring::evaluate(const SharedCarState_s &state) { tick(state.systick, state.pedals_data, state.loadcell_data); return writeout_; diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/SimpleController.cpp index 3ffc42549..08e308680 100644 --- a/lib/systems/src/SimpleController.cpp +++ b/lib/systems/src/SimpleController.cpp @@ -40,7 +40,7 @@ void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) } } -TorqueControllerOutput_s TorqueControllerSimple::evaluate(const car_state &state) +TorqueControllerOutput_s TorqueControllerSimple::evaluate(const SharedCarState_s &state) { tick(state.pedals_data); return writeout_; diff --git a/src/main.cpp b/src/main.cpp index 7154a04c6..2a246275f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -348,7 +348,7 @@ void loop() // single source of truth for the state of the car. // no systems or interfaces should write directly to this. - car_state car_state_inst(curr_tick, + SharedCarState_s car_state_inst(curr_tick, steering_system.getSteeringSystemData(), drivetrain.get_dynamic_data(), load_cell_interface.getLoadCellForces(), diff --git a/test/test_systems/fake_controller_type.h b/test/test_systems/fake_controller_type.h index 93d5cc2b4..becdb85ab 100644 --- a/test/test_systems/fake_controller_type.h +++ b/test/test_systems/fake_controller_type.h @@ -10,7 +10,6 @@ class TestControllerType : public virtual Controller public: TorqueControllerOutput_s output; - TorqueControllerOutput_s evaluate(const car_state &state) override { return output; } - // void update_input_state(const car_state &state) override { } + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { return output; } }; #endif // __FAKE_CONTROLLER_TYPE_H__ \ No newline at end of file diff --git a/test/test_systems/launch_controller_integration_testing.h b/test/test_systems/launch_controller_integration_testing.h index dcf667450..b7a52b6b5 100644 --- a/test/test_systems/launch_controller_integration_testing.h +++ b/test/test_systems/launch_controller_integration_testing.h @@ -48,12 +48,12 @@ TEST(LaunchIntergationTesting, test_simple_launch_controller) SysClock clock = SysClock(); SysTick_s cur_tick; cur_tick = clock.tick(0); - car_state still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}); - car_state pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}); - car_state no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); - car_state barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); - car_state one_sec_passed_in_launch_state(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); - car_state one_sec_passed_in_launch_state_w_error(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_accel_and_brake_press, {}); + SharedCarState_s still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}); + SharedCarState_s pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s one_sec_passed_in_launch_state(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s one_sec_passed_in_launch_state_w_error(clock.tick(1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_accel_and_brake_press, {}); // mode 0 TorqueControllerSimple tc_simple(1.0f, 1.0f); @@ -135,26 +135,26 @@ TEST(LaunchIntergationTesting, test_slip_launch_controller) static_cast(&slip_launch)}, {false, false, true, false, false}); - car_state still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}); - car_state pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}); - car_state no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); - car_state barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s still_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_no_accel_press, {}); + SharedCarState_s pedal_pressed_state(cur_tick, {}, simulated_slow_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s no_launch_allowed_state(cur_tick, {}, simulated_no_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s barely_launch_state(cur_tick, {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); // getting to slip launch (mode 4) res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY); - car_state one_hundredth_sec_passed_in_launch_state(clock.tick(10000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s one_hundredth_sec_passed_in_launch_state(clock.tick(10000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, one_hundredth_sec_passed_in_launch_state); - car_state two_hundredth_sec_passed_in_launch_state(clock.tick(20000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s two_hundredth_sec_passed_in_launch_state(clock.tick(20000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, two_hundredth_sec_passed_in_launch_state); ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); - car_state more_accel_time_since_launch(clock.tick( BaseLaunchControllerParams::const_accel_time * 1000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); + SharedCarState_s more_accel_time_since_launch(clock.tick( BaseLaunchControllerParams::const_accel_time * 1000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, {}); res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, more_accel_time_since_launch); ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); @@ -162,7 +162,7 @@ TEST(LaunchIntergationTesting, test_slip_launch_controller) // // if velocity is less than the default speed, it should still go at launch speed vn_data.velocity_x = 0; // m/s - car_state small_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data); + SharedCarState_s small_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 1000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data); res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, small_vn_vel); printf("lower vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target @@ -172,7 +172,7 @@ TEST(LaunchIntergationTesting, test_slip_launch_controller) // // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this vn_data.velocity_x = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s - car_state big_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 2000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data); + SharedCarState_s big_vn_vel(clock.tick(( BaseLaunchControllerParams::const_accel_time * 1000) + 2000000), {}, simulated_barely_launch_drivetrain_dynamics, {}, simulated_full_accel_press, vn_data); res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, big_vn_vel); ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); diff --git a/test/test_systems/state_machine_test.h b/test/test_systems/state_machine_test.h index 7e63b6369..2e307d5e4 100644 --- a/test/test_systems/state_machine_test.h +++ b/test/test_systems/state_machine_test.h @@ -10,7 +10,7 @@ class FakeTCMux public: DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e requested_torque_limit, - const car_state &input_state) { return {}; } + const SharedCarState_s &input_state) { return {}; } }; class DrivetrainMock @@ -35,7 +35,7 @@ class DrivetrainMock void disable_no_pins(){}; }; -car_state dummy_state({}, {}, {}, {}, {}, {}); +SharedCarState_s dummy_state({}, {}, {}, {}, {}, {}); void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) { diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 97fa896c7..fbd074a56 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -52,7 +52,7 @@ TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) { TestControllerType inst1, inst2; TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - car_state state({}, {}, {}, {}, {}, {}); + 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); @@ -73,7 +73,7 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) set_outputs(inst1, 0, 1); set_outputs(inst2, 6, 1); TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - car_state state({}, {}, {}, {}, {}, {}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); set_four_outputs(state.drivetrain_data.measuredSpeeds, 10000.0); state.pedals_data = {}; @@ -103,7 +103,7 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) set_outputs(inst1, 0.1, 1); set_outputs(inst2, 3, 10); TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - car_state state({}, {}, {}, {}, {}, {}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); 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); @@ -176,7 +176,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) static_cast(&simple_launch), static_cast(&slip_launch)}, {false, true, false, false, false}); - car_state mode_0_input_state({}, {}, {}, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + 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); @@ -205,7 +205,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) { drivetrain_data.measuredSpeeds[i] = 500.0f; } - car_state mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + SharedCarState_s mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); DrivetrainCommand_s res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); @@ -219,7 +219,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) } set_output_rpm(inst1, 20000, 21.0); - car_state mode_0_input_state_high_power({}, {}, drivetrain_data, {}, {.accelPercent = 1.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + SharedCarState_s mode_0_input_state_high_power({}, {}, drivetrain_data, {}, {.accelPercent = 1.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state_high_power); for (int i = 0; i < 4; i++) @@ -243,7 +243,7 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) drivetrain_data.measuredSpeeds[i] = 500.0f; } - car_state mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); + SharedCarState_s mode_0_input_state({}, {}, drivetrain_data, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); auto drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); From adc599c9a2a3d1afe4ee22fcad0fd1116c474500 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Tue, 14 May 2024 21:29:54 -0400 Subject: [PATCH 12/31] adding in printlogger, did on car testing. currently some issue exists with the torque mode switching --- include/PrintLogger.h | 44 +++++++++++++++++++ lib/interfaces/include/DashboardInterface.h | 1 + lib/interfaces/src/DashboardInterface.cpp | 8 +++- lib/state_machine/include/MCUStateMachine.h | 2 + lib/state_machine/include/MCUStateMachine.tpp | 3 +- lib/systems/include/TorqueControllerMux.h | 3 ++ src/main.cpp | 11 ++--- 7 files changed, 63 insertions(+), 9 deletions(-) create mode 100644 include/PrintLogger.h diff --git a/include/PrintLogger.h b/include/PrintLogger.h new file mode 100644 index 000000000..fad214d1d --- /dev/null +++ b/include/PrintLogger.h @@ -0,0 +1,44 @@ +#ifndef __PRINTLOGGER_H__ +#define __PRINTLOGGER_H__ +#include +#include +#ifdef ARDUINO +#include +#else +#include +#include +#include +#endif + +#ifdef ARDUINO +template +void logger_println(T out){ + Serial.println(out); +} +#else +template +void logger_println(T out){ + std::cout << out< + void log_out(T out, unsigned long current_millis, unsigned long period=100) { + if((current_millis - last_out_time_) > period) + { + logger_println(out); + last_out_time_ = current_millis; + } + } +}; +#endif // __PRINTLOGGER_H__ \ No newline at end of file diff --git a/lib/interfaces/include/DashboardInterface.h b/lib/interfaces/include/DashboardInterface.h index b5a4e33d9..082164dad 100644 --- a/lib/interfaces/include/DashboardInterface.h +++ b/lib/interfaces/include/DashboardInterface.h @@ -88,6 +88,7 @@ class DashboardInterface DashComponentInterface_s _data; bool prev_button_pressed_state_; void update_torque_mode_(bool button_pressed); + public: /*! Constructor for new DashboardInterface, All that it is inited with diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index 22210a81e..8ea8d8699 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -30,9 +30,15 @@ void DashboardInterface::read(const CAN_message_t &can_msg) void DashboardInterface::update_torque_mode_(bool button_pressed) { // detect high-to-low transition - + Serial.println("prev button state: "); + Serial.println(prev_button_pressed_state_); + Serial.println("button pressed: "); + Serial.println(button_pressed); + Serial.println("torque mode limit"); + Serial.println(static_cast(_data.torque_limit_mode)); if (prev_button_pressed_state_ == true && button_pressed == false) { + _data.torque_limit_mode = static_cast((static_cast(_data.torque_limit_mode) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); } prev_button_pressed_state_ = button_pressed; diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index 45ae41e74..ad54bab95 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -11,6 +11,7 @@ #include "SafetySystem.h" #include "DashboardInterface.h" #include "AMSInterface.h" +#include "PrintLogger.h" // #include "IMDInterface.h" @@ -75,6 +76,7 @@ class MCUStateMachine // IMDInterface *imd_; SafetySystem *safety_system_; TCMuxType *controller_mux_; + RateLimitedLogger logger_; }; #include "MCUStateMachine.tpp" #endif /* MCUSTATEMACHINE */ diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index caddf9fad..9e75321b1 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -144,7 +144,8 @@ void MCUStateMachine::tick_state_machine(unsigned if (safety_system_->get_software_is_ok() && !data.implausibilityExceededMaxDuration) { - + logger_.log_out("torque mode:", current_millis, 100); + logger_.log_out(static_cast(dashboard_->getTorqueLimitMode()), current_millis, 100); drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state)); } else diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 24152c02b..a298ed19b 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -33,6 +33,9 @@ // - [x] add 3 bit status to a telemetry message for the TC mux status to HT_CAN // - [x] pass state of tc mux into telem interface and add the CAN signal // - [x] remove the old tc mux +// - [ ] add back checking of the ready flag of the controllers and if the controller isnt ready it defaults +// - [ ] add test for this +// - [ ] make folder for the controllers // - [ ] write integration tests for the real controllers // - [x] test construction with real controllers // - [ ] ensure that sane outputs occur on first tick of each controller diff --git a/src/main.cpp b/src/main.cpp index 2a246275f..0a75b123e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -39,6 +39,7 @@ #include "MCUStateMachine.h" #include "HT08_CASE.h" +#include "PrintLogger.h" /* PARAMETER STRUCTS */ @@ -210,7 +211,7 @@ CASEConfiguration case_config = { .max_regen_torque = 21.42, .max_torque = 21.42, }; - +RateLimitedLogger logger; //// Controllers CASESystem case_system(&CAN3_txBuffer, 100, 70, 550, case_config); // mode 0 @@ -270,11 +271,6 @@ void setup() // protobuf_send_socket.begin(EthParams::default_protobuf_send_port); // protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); - /* Do this to send message VVV */ - // protobuf_socket.beginPacket(EthParams::default_TCU_ip, EthParams::default_protobuf_port); - // protobuf_socket.write(buf, len); - // protobuf_socker.endPacket(); - SPI.begin(); a1.init(); a2.init(); @@ -356,7 +352,8 @@ void loop() vn_interface.get_vn_struct()); tick_all_systems(curr_tick); - + + // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode_), curr_tick.millis, 100); // inverter procedure before entering state machine // reset inverters if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) From 3bb36b4f994bbacbcf1bdf77165e775a07a92715 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 13 Sep 2024 19:44:13 -0400 Subject: [PATCH 13/31] updating pedals systems test, version of shared firmware interfaces and to the latest pedals parameters --- include/MCU_rev15_defs.h | 16 +++++++------- platformio.ini | 2 +- test/test_systems/pedals_system_test.h | 29 ++++++++++++++++---------- 3 files changed, 27 insertions(+), 20 deletions(-) diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index e2399cb33..a11ee99dc 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -52,17 +52,17 @@ const unsigned long TELEM_CAN_BAUDRATE = 500000; // All of these values are the PEDAL min/max // the sensor min/max that trip implaus are calculated // in the PedalsSystem constructor -const int ACCEL1_PEDAL_MAX = 3409; // 3330 -const int ACCEL2_PEDAL_MAX = 259; // 388 +const int ACCEL1_PEDAL_MAX = 3491; +const int ACCEL2_PEDAL_MAX = 189; -const int ACCEL1_PEDAL_MIN = 2129; -const int ACCEL2_PEDAL_MIN = 1517; +const int ACCEL1_PEDAL_MIN = 2244; +const int ACCEL2_PEDAL_MIN = 1405; -const int BRAKE1_PEDAL_MAX = 1945; // 2200; -const int BRAKE2_PEDAL_MAX = 1742; // 2200; +const int BRAKE1_PEDAL_MAX = 1945; +const int BRAKE2_PEDAL_MAX = 1742; -const int BRAKE1_PEDAL_MIN = 1230; // 785; // 1230 to 1750 -const int BRAKE2_PEDAL_MIN = 2437; // 785; // 2450 to 1930 +const int BRAKE1_PEDAL_MIN = 1192; +const int BRAKE2_PEDAL_MIN = 2476; const int ACCEL1_PEDAL_OOR_MIN = 90; const int ACCEL2_PEDAL_OOR_MIN = 90; diff --git a/platformio.ini b/platformio.ini index e1b05a785..9de9847a3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -55,7 +55,7 @@ extra_scripts = pre:extra_script.py lib_deps = ${common.lib_deps_shared} SPI - https://github.com/hytech-racing/shared_firmware_interfaces.git#feature/thermistor-template + https://github.com/hytech-racing/shared_firmware_interfaces.git#shared_types https://github.com/tonton81/FlexCAN_T4 https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/RCMast3r/hytech_can#testing_new_inv_ids diff --git a/test/test_systems/pedals_system_test.h b/test/test_systems/pedals_system_test.h index 36fa6a812..dc0f19a1c 100644 --- a/test/test_systems/pedals_system_test.h +++ b/test/test_systems/pedals_system_test.h @@ -273,12 +273,15 @@ TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activati TEST(PedalsSystemTesting, test_implausibility_duration) { AnalogConversion_s test_accel1_val = {2583, 0.37, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_accel2_val = {1068, 0.37, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_brake_val = {1510, get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, 1510), AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel2_val = {1068, 1.00, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {1510, get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, BRAKE1_PEDAL_MAX), AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; PedalsSystem pedals(get_real_accel_pedal_params(), get_real_brake_pedal_params()); auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + + std::cout << data.accelPressed < Date: Fri, 13 Sep 2024 19:48:45 -0400 Subject: [PATCH 14/31] removing non-used function --- lib/systems/include/BaseController.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index 5ae447050..a94556fb9 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -10,10 +10,6 @@ namespace BaseControllerParams } class Controller { -private: - // common function for calculation of the limited torque request that goes into controllers - float get_torque_request_(float torque_limit_nm, const PedalsSystemData_s &pedals_data) { return 0; }; - public: virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; }; From 7458e73d9a38d05cf68b0545316042fb651ddf97 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 00:12:41 -0400 Subject: [PATCH 15/31] working on ca --- lib/interfaces/include/HytechCANInterface.h | 2 +- lib/interfaces/include/ParameterInterface.h | 46 -------- lib/interfaces/include/ProtobufMsgInterface.h | 27 ++--- lib/interfaces/include/VectornavInterface.tpp | 10 +- lib/interfaces/src/TelemetryInterface.cpp | 24 ++-- lib/state_machine/include/MCUStateMachine.h | 2 - lib/systems/include/CASESystem.h | 108 +++++++++--------- platformio.ini | 3 +- src/main.cpp | 27 ++--- 9 files changed, 87 insertions(+), 162 deletions(-) delete mode 100644 lib/interfaces/include/ParameterInterface.h diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 7e1c47235..77a250cc1 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -170,7 +170,7 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_LAT_LON_CANID: interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg); break; - case VN_GPS_TIME_CANID: + case VN_GPS_TIME_MSG_CANID: interfaces.vn_interface->retrieve_gps_time_CAN(recvd_msg); break; case VN_STATUS_CANID: diff --git a/lib/interfaces/include/ParameterInterface.h b/lib/interfaces/include/ParameterInterface.h deleted file mode 100644 index c6b0cac1c..000000000 --- a/lib/interfaces/include/ParameterInterface.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef PARAMETERINTERFACE -#define PARAMETERINTERFACE -#include "MCUStateMachine.h" -#include "ht_eth.pb.h" -#include "default_config.h" - -// yes, i know this is a singleton. im prototyping rn. -// TODO review if I can just give this a pointer to an ethernet port -class ParameterInterface -{ -public: - ParameterInterface(): current_car_state_(CAR_STATE::STARTUP), params_need_sending_(false), config_(DEFAULT_CONFIG) {} - - void update_car_state(const CAR_STATE& state) - { - current_car_state_ = state; - } - void update_config(const config &config) - { - if(static_cast(current_car_state_) < 5 ){ - config_ = config; - } - - } - config get_config() - { - return config_; - } - void set_params_need_sending() - { - params_need_sending_ = true; - } - void reset_params_need_sending() - { - params_need_sending_ = false; - } - bool params_need_sending() { return params_need_sending_; } - -private: - CAR_STATE current_car_state_; - bool params_need_sending_ = false; - config config_; - -}; - -#endif \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index d360458d3..05e660442 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -1,11 +1,9 @@ #ifndef PROTOBUFMSGINTERFACE #define PROTOBUFMSGINTERFACE -#include "ht_eth.pb.h" #include "pb_encode.h" #include "pb_decode.h" #include "pb_common.h" -#include "ParameterInterface.h" #include "circular_buffer.h" #include "NativeEthernet.h" #include "MCU_rev15_defs.h" @@ -13,7 +11,6 @@ struct ETHInterfaces { - ParameterInterface* param_interface; }; using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); @@ -50,27 +47,17 @@ bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, c return true; } -// -void recv_pb_stream_union_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces) + +template +std::pair recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces, const pb_msgdesc_t * desc_pointer) { pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); - HT_ETH_Union msg = HT_ETH_Union_init_zero; - if (pb_decode(&stream, HT_ETH_Union_fields, &msg)) + pb_msg_type msg = {}; + if (pb_decode(&stream, desc_pointer, &msg)) { - Serial.println("decoded!"); - - switch (msg.which_type_union) - { - case HT_ETH_Union_config__tag: - interfaces.param_interface->update_config(msg.type_union.config_); - break; - case HT_ETH_Union_get_config__tag: - interfaces.param_interface->set_params_need_sending(); - break; - default: - break; - } + return {msg, true}; } + return {msg, }; } diff --git a/lib/interfaces/include/VectornavInterface.tpp b/lib/interfaces/include/VectornavInterface.tpp index 011791ccf..aa0bc6a2c 100644 --- a/lib/interfaces/include/VectornavInterface.tpp +++ b/lib/interfaces/include/VectornavInterface.tpp @@ -50,8 +50,8 @@ void VNInterface::retrieve_lat_lon_CAN(CAN_message_t &recvd_msg) template void VNInterface::retrieve_gps_time_CAN(CAN_message_t &recvd_msg) { - VN_GPS_TIME_t gps_time_data; - Unpack_VN_GPS_TIME_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len); + VN_GPS_TIME_MSG_t gps_time_data; + Unpack_VN_GPS_TIME_MSG_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len); vn_data.gps_time = gps_time_data.vn_gps_time; } @@ -72,9 +72,9 @@ void VNInterface::retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recv template void VNInterface::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) { - VN_ECEF_POS_Z_t ecef_z; - Unpack_VN_ECEF_POS_Z_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); - vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro); + VN_ECEF_POS_Z_MSG_t ecef_z; + Unpack_VN_ECEF_POS_Z_MSG_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); + vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_msg_ro_fromS(ecef_z.vn_ecef_pos_z_ro); } template diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index ba61f3f1e..d2d7d2bdd 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -177,10 +177,10 @@ void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg( // TODO: change this to use actual torque values from inverter // Torque current just temporary for gearbox seal validation DRIVETRAIN_TORQUE_TELEM_t torque; - torque.fl_motor_torque_ro = HYTECH_fl_motor_torque_ro_toS(fl->get_torque_current()); - torque.fr_motor_torque_ro = HYTECH_fr_motor_torque_ro_toS(fr->get_torque_current()); - torque.rl_motor_torque_ro = HYTECH_rl_motor_torque_ro_toS(rl->get_torque_current()); - torque.rr_motor_torque_ro = HYTECH_rr_motor_torque_ro_toS(rr->get_torque_current()); + torque.fl_motor_torque = fl->get_torque_current(); + torque.fr_motor_torque = fr->get_torque_current(); + torque.rl_motor_torque = rl->get_torque_current(); + torque.rr_motor_torque = rr->get_torque_current(); enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech); } @@ -219,16 +219,16 @@ void TelemetryInterface::update_steering_status_CAN_msg(const float steering_sys const uint8_t steering_encoder_status, const uint8_t steering_analog_status) { - // STEERING_SYSTEM_REPORT_t msg; + STEERING_SYSTEM_REPORT_t msg; - // msg.steering_system_angle_ro = HYTECH_steering_system_angle_ro_toS(steering_system_angle); - // msg.steering_encoder_angle_ro = HYTECH_steering_encoder_angle_ro_toS(filtered_angle_encoder); - // msg.steering_analog_angle_ro = HYTECH_steering_analog_angle_ro_toS(filtered_angle_analog); - // msg.steering_system_status = steering_system_status; - // msg.steering_encoder_status = steering_encoder_status; - // msg.steering_analog_status = steering_analog_status; + msg.steering_system_angle_ro = HYTECH_steering_system_angle_ro_toS(steering_system_angle); + msg.steering_encoder_angle_ro = HYTECH_steering_encoder_angle_ro_toS(filtered_angle_encoder); + msg.steering_analog_angle_ro = HYTECH_steering_analog_angle_ro_toS(filtered_angle_analog); + msg.steering_system_status = steering_system_status; + msg.steering_encoder_status = steering_encoder_status; + msg.steering_analog_status = steering_analog_status; - // enqueue_new_CAN(&msg, &Pack_STEERING_SYSTEM_REPORT_hytech); + enqueue_new_CAN(&msg, &Pack_STEERING_SYSTEM_REPORT_hytech); } /* Send CAN messages */ diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index ad54bab95..db6707f21 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -13,8 +13,6 @@ #include "AMSInterface.h" #include "PrintLogger.h" -// #include "IMDInterface.h" - enum class CAR_STATE { STARTUP = 0, diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index aae92fd24..3349d9488 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -8,7 +8,7 @@ #include "SteeringSystem.h" #include "MCUStateMachine.h" #include "ProtobufMsgInterface.h" -#include "ParameterInterface.h" +// #include "ParameterInterface.h" struct CASEConfiguration { @@ -167,59 +167,59 @@ class CASESystem /// @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 - void update_config_from_param_interface(ParameterInterface ¶m_interface_ref) - { - config cfg = param_interface_ref.get_config(); - config_.AbsoluteTorqueLimit = cfg.AbsoluteTorqueLimit; - config_.yaw_pid_p = cfg.yaw_pid_p; - config_.yaw_pid_i = cfg.yaw_pid_i; - config_.yaw_pid_d = cfg.yaw_pid_d; - config_.tcs_pid_p_lowerBound_front = cfg.tcs_pid_p_lowerBound_front; - config_.tcs_pid_p_upperBound_front = cfg.tcs_pid_p_upperBound_front; - config_.tcs_pid_p_lowerBound_rear = cfg.tcs_pid_p_lowerBound_rear; - config_.tcs_pid_p_upperBound_rear = cfg.tcs_pid_p_upperBound_rear; - config_.tcs_pid_i = cfg.tcs_pid_i; - config_.tcs_pid_d = cfg.tcs_pid_d; - config_.useLaunch = cfg.useLaunch; - config_.usePIDTV = cfg.usePIDTV; - config_.useTCSLimitedYawPID = cfg.useTCSLimitedYawPID; - config_.useNormalForce = cfg.useNormalForce; - config_.useTractionControl = cfg.useTractionControl; - config_.usePowerLimit = cfg.usePowerLimit; - config_.usePIDPowerLimit = cfg.usePIDPowerLimit; - config_.useDecoupledYawBrakes = cfg.useDecoupledYawBrakes; - config_.useDiscontinuousYawPIDBrakes = cfg.useDiscontinuousYawPIDBrakes; - config_.tcsSLThreshold = cfg.tcsSLThreshold; - config_.launchSL = cfg.launchSL; - config_.launchDeadZone = cfg.launchDeadZone; - config_.launchVelThreshold = cfg.launchVelThreshold; - config_.tcsVelThreshold = cfg.tcsVelThreshold; - config_.yawPIDMaxDifferential = cfg.yawPIDMaxDifferential; - config_.yawPIDErrorThreshold = cfg.yawPIDErrorThreshold; - config_.yawPIDVelThreshold = cfg.yawPIDVelThreshold; - config_.yawPIDCoastThreshold = cfg.yawPIDCoastThreshold; - config_.yaw_pid_brakes_p = cfg.yaw_pid_brakes_p; - config_.yaw_pid_brakes_i = cfg.yaw_pid_brakes_i; - config_.yaw_pid_brakes_d = cfg.yaw_pid_brakes_d; - config_.decoupledYawPIDBrakesMaxDIfference = cfg.decoupledYawPIDBrakesMaxDIfference; - config_.discontinuousBrakesPercentThreshold = cfg.discontinuousBrakesPercentThreshold; - config_.TorqueMode = cfg.TorqueMode; - config_.RegenLimit = cfg.RegenLimit; - config_.useNoRegen5kph = cfg.useNoRegen5kph; - config_.useTorqueBias = cfg.useTorqueBias; - config_.DriveTorquePercentFront = cfg.DriveTorquePercentFront; - config_.BrakeTorquePercentFront = cfg.BrakeTorquePercentFront; - config_.MechPowerMaxkW = cfg.MechPowerMaxkW; - config_.launchLeftRightMaxDiff = cfg.launchLeftRightMaxDiff; - config_.tcs_pid_lower_rpm_front = cfg.tcs_pid_lower_rpm_front; - config_.tcs_pid_upper_rpm_front = cfg.tcs_pid_upper_rpm_front; - config_.tcs_pid_lower_rpm_rear = cfg.tcs_pid_lower_rpm_rear; - config_.tcs_pid_upper_rpm_rear = cfg.tcs_pid_upper_rpm_rear; - config_.maxNormalLoadBrakeScalingFront = cfg.maxNormalLoadBrakeScalingFront; - config_.max_rpm = cfg.max_rpm; - config_.max_regen_torque = cfg.max_regen_torque; - config_.max_torque = cfg.max_torque; - } + // void update_config_from_param_interface(ParameterInterface ¶m_interface_ref) + // { + // config cfg = param_interface_ref.get_config(); + // config_.AbsoluteTorqueLimit = cfg.AbsoluteTorqueLimit; + // config_.yaw_pid_p = cfg.yaw_pid_p; + // config_.yaw_pid_i = cfg.yaw_pid_i; + // config_.yaw_pid_d = cfg.yaw_pid_d; + // config_.tcs_pid_p_lowerBound_front = cfg.tcs_pid_p_lowerBound_front; + // config_.tcs_pid_p_upperBound_front = cfg.tcs_pid_p_upperBound_front; + // config_.tcs_pid_p_lowerBound_rear = cfg.tcs_pid_p_lowerBound_rear; + // config_.tcs_pid_p_upperBound_rear = cfg.tcs_pid_p_upperBound_rear; + // config_.tcs_pid_i = cfg.tcs_pid_i; + // config_.tcs_pid_d = cfg.tcs_pid_d; + // config_.useLaunch = cfg.useLaunch; + // config_.usePIDTV = cfg.usePIDTV; + // config_.useTCSLimitedYawPID = cfg.useTCSLimitedYawPID; + // config_.useNormalForce = cfg.useNormalForce; + // config_.useTractionControl = cfg.useTractionControl; + // config_.usePowerLimit = cfg.usePowerLimit; + // config_.usePIDPowerLimit = cfg.usePIDPowerLimit; + // config_.useDecoupledYawBrakes = cfg.useDecoupledYawBrakes; + // config_.useDiscontinuousYawPIDBrakes = cfg.useDiscontinuousYawPIDBrakes; + // config_.tcsSLThreshold = cfg.tcsSLThreshold; + // config_.launchSL = cfg.launchSL; + // config_.launchDeadZone = cfg.launchDeadZone; + // config_.launchVelThreshold = cfg.launchVelThreshold; + // config_.tcsVelThreshold = cfg.tcsVelThreshold; + // config_.yawPIDMaxDifferential = cfg.yawPIDMaxDifferential; + // config_.yawPIDErrorThreshold = cfg.yawPIDErrorThreshold; + // config_.yawPIDVelThreshold = cfg.yawPIDVelThreshold; + // config_.yawPIDCoastThreshold = cfg.yawPIDCoastThreshold; + // config_.yaw_pid_brakes_p = cfg.yaw_pid_brakes_p; + // config_.yaw_pid_brakes_i = cfg.yaw_pid_brakes_i; + // config_.yaw_pid_brakes_d = cfg.yaw_pid_brakes_d; + // config_.decoupledYawPIDBrakesMaxDIfference = cfg.decoupledYawPIDBrakesMaxDIfference; + // config_.discontinuousBrakesPercentThreshold = cfg.discontinuousBrakesPercentThreshold; + // config_.TorqueMode = cfg.TorqueMode; + // config_.RegenLimit = cfg.RegenLimit; + // config_.useNoRegen5kph = cfg.useNoRegen5kph; + // config_.useTorqueBias = cfg.useTorqueBias; + // config_.DriveTorquePercentFront = cfg.DriveTorquePercentFront; + // config_.BrakeTorquePercentFront = cfg.BrakeTorquePercentFront; + // config_.MechPowerMaxkW = cfg.MechPowerMaxkW; + // config_.launchLeftRightMaxDiff = cfg.launchLeftRightMaxDiff; + // config_.tcs_pid_lower_rpm_front = cfg.tcs_pid_lower_rpm_front; + // config_.tcs_pid_upper_rpm_front = cfg.tcs_pid_upper_rpm_front; + // config_.tcs_pid_lower_rpm_rear = cfg.tcs_pid_lower_rpm_rear; + // config_.tcs_pid_upper_rpm_rear = cfg.tcs_pid_upper_rpm_rear; + // config_.maxNormalLoadBrakeScalingFront = cfg.maxNormalLoadBrakeScalingFront; + // config_.max_rpm = cfg.max_rpm; + // config_.max_regen_torque = cfg.max_regen_torque; + // config_.max_torque = cfg.max_torque; + // } float get_rpm_setpoint(float final_torque) { if (final_torque > 0) diff --git a/platformio.ini b/platformio.ini index 9de9847a3..75986562f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -2,7 +2,6 @@ lib_deps_shared = Nanopb git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 - https://github.com/hytech-racing/HT_params/releases/download/2024-05-05T13_33_07/ht_eth_pb_lib.tar.gz https://github.com/hytech-racing/shared_firmware_types.git#feb3b83 https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 @@ -59,7 +58,7 @@ lib_deps = https://github.com/tonton81/FlexCAN_T4 https://github.com/ssilverman/QNEthernet#v0.26.0 https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/90/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/121/can_lib.tar.gz [env:test_can_on_teensy] lib_ignore = diff --git a/src/main.cpp b/src/main.cpp index f34aa013c..342034de0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,7 @@ /* Include files */ /* System Includes*/ #include -#include "ParameterInterface.h" +// #include "ParameterInterface.h" /* Libraries */ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" @@ -175,8 +175,7 @@ OrbisBR10 steering1(&Serial5); // /* // INTERFACES // */ -ParameterInterface param_interface; -ETHInterfaces ethernet_interfaces = {¶m_interface}; +ETHInterfaces ethernet_interfaces = {}; VNInterface vn_interface(&CAN3_txBuffer); DashboardInterface dashboard(&CAN3_txBuffer); AMSInterface ams_interface(&CAN3_txBuffer, SOFTWARE_OK); @@ -446,9 +445,6 @@ void loop() fsm.tick_state_machine(curr_tick.millis, car_state_inst); - // give the state of the car to the param interface - param_interface.update_car_state(fsm.get_state()); - // tick safety system safety_system.software_shutdown(curr_tick); @@ -492,8 +488,11 @@ void loop() Serial.println(ams_interface.get_filtered_min_cell_voltage()); 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.print("Current TC error: "); + Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); Serial.println(); - Serial.println(); } @@ -677,7 +676,6 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) dashboard.startButtonPressed(), vn_interface.get_vn_struct().vn_status); - // case_system.update_config_from_param_interface(param_interface); } void handle_ethernet_interface_comms() @@ -686,17 +684,6 @@ void handle_ethernet_interface_comms() // via the union message. this is a little bit cursed ngl. // TODO un fuck this and make it more sane // Serial.println("bruh"); - handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); + // handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); - // this is just kinda here i know. - if (param_interface.params_need_sending()) - { - // Serial.println("handling ethernet"); - auto config = param_interface.get_config(); - if (!handle_ethernet_socket_send_pb(&protobuf_send_socket, config, config_fields)) - { - // TODO this means that something bad has happend - } - param_interface.reset_params_need_sending(); - } } \ No newline at end of file From b8fb3fbab672481aa141b8b723fb9212a9890723 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 00:15:36 -0400 Subject: [PATCH 16/31] removing commented code --- lib/interfaces/src/TelemetryInterface.cpp | 29 ----------------------- 1 file changed, 29 deletions(-) diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index d2d7d2bdd..e96a0b6cf 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -58,16 +58,6 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon enqueue_CAN(mcu_analog_readings_, ID_MCU_ANALOG_READINGS); } -// void TelemetryInterface::update_front_thermistors_CAN_msg(const AnalogConversion_s &therm_fl, -// const AnalogConversion_s &therm_fr) -// { - -// FRONT_THERMISTORS_t front_thermistors_; -// front_thermistors_.thermistor_motor_fl = therm_fl.raw; -// front_thermistors_.thermistor_motor_fr = therm_fr.raw; - -// enqueue_new_CAN(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); -// } void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, InvInt_t *rr) { @@ -195,23 +185,6 @@ void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s enqueue_new_CAN(&message, &Pack_PENTHOUSE_ACCUM_MSG_hytech); } -// void TelemetryInterface::update_TCMux_status_CAN_msg(const TCMuxStatus_s &tcMuxStatus) -// { -// TCMUX_STATUS_REPORT_t msg; - -// msg.speed_above_thresh = tcMuxStatus.speedPreventsModeChange; -// msg.torque_delta_above_thresh = tcMuxStatus.torqueDeltaPreventsModeChange; -// msg.tc_not_ready = tcMuxStatus.controllerNotReadyPreventsModeChange; -// msg.steering_system_has_err = tcMuxStatus.steeringSystemError; -// msg.mode_intended = tcMuxStatus.modeIntended; -// msg.mode_actual = tcMuxStatus.modeActual; -// msg.dash_dial_mode = tcMuxStatus.dialMode; -// msg.torque_mode = tcMuxStatus.torqueMode; -// msg.torque_limit_ro = HYTECH_torque_limit_ro_toS(tcMuxStatus.maxTorque); - -// enqueue_new_CAN(&msg, &Pack_TCMUX_STATUS_REPORT_hytech); -// } - void TelemetryInterface::update_steering_status_CAN_msg(const float steering_system_angle, const float filtered_angle_encoder, const float filtered_angle_analog, @@ -312,6 +285,4 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel], adc1.conversions[channels_.current_ref_channel]); - // update_front_thermistors_CAN_msg(mcu_adc.conversions[channels_.therm_fl_channel], - // mcu_adc.conversions[channels_.therm_fr_channel]); } \ No newline at end of file From 79637f1e9797240e0d5f51735bbd542986bf4d4a Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 00:26:11 -0400 Subject: [PATCH 17/31] one line does one thing and handling const correctly --- lib/interfaces/include/ThermistorInterface.h | 2 +- lib/interfaces/src/ThermistorInterface.cpp | 21 +++++++++++--------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/lib/interfaces/include/ThermistorInterface.h b/lib/interfaces/include/ThermistorInterface.h index 4c49f3ec6..4af4c5973 100644 --- a/lib/interfaces/include/ThermistorInterface.h +++ b/lib/interfaces/include/ThermistorInterface.h @@ -16,7 +16,7 @@ const int MCU15_THERM_FR_CHANNEL = 1; class ThermistorInterface { private: - Thermistors front_thermistors; + Thermistors _front_thermistors; float therm_fl; float therm_fr; diff --git a/lib/interfaces/src/ThermistorInterface.cpp b/lib/interfaces/src/ThermistorInterface.cpp index 642f01046..29ac6d267 100644 --- a/lib/interfaces/src/ThermistorInterface.cpp +++ b/lib/interfaces/src/ThermistorInterface.cpp @@ -3,22 +3,21 @@ -ThermistorInterface::ThermistorInterface(CANBufferType *msg_output_queue) : front_thermistors() +ThermistorInterface::ThermistorInterface(CANBufferType *msg_output_queue) { _msg_queue = msg_output_queue; } - void ThermistorInterface::update_front_thermistor_readings() { - // FRONT_THERMISTORS_t front_thermistors_; - // //scale by 500 for easy packing - // front_thermistors_.thermistor_motor_fl = HYTECH_thermistor_motor_fl_toS(therm_fl); - // front_thermistors_.thermistor_motor_fr = HYTECH_thermistor_motor_fr_toS(therm_fr); + FRONT_THERMISTORS_t front_thermistors; + //scale by 500 for easy packing + front_thermistors.thermistor_motor_fl_ro = HYTECH_thermistor_motor_fl_ro_toS(therm_fl); + front_thermistors.thermistor_motor_fr_ro = HYTECH_thermistor_motor_fr_ro_toS(therm_fr); - // enqueue_CAN_front_thermistors(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); + enqueue_CAN_front_thermistors(&front_thermistors, &Pack_FRONT_THERMISTORS_hytech); } template @@ -32,9 +31,13 @@ void ThermistorInterface::enqueue_CAN_front_thermistors(U* structure, uint32_t ( } void ThermistorInterface::tick(const AnalogConversion_s &raw_therm_fl, const AnalogConversion_s &raw_therm_fr) + { - therm_fl = front_thermistors.get(MCU15_THERM_FL_CHANNEL).convert(raw_therm_fl.raw); - therm_fr = front_thermistors.get(MCU15_THERM_FR_CHANNEL).convert(raw_therm_fr.raw); + auto fl_channel = _front_thermistors.get(MCU15_THERM_FL_CHANNEL); + auto fr_channel = _front_thermistors.get(MCU15_THERM_FR_CHANNEL); + + therm_fl = fl_channel.convert((uint16_t)raw_therm_fl.raw); + therm_fr = fr_channel.convert((uint16_t)raw_therm_fr.raw); update_front_thermistor_readings(); } \ No newline at end of file From f7768c445a941d2dc00fe9cc52f2e66cbf447894 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 00:36:29 -0400 Subject: [PATCH 18/31] refactoring name of vectornav struct --- lib/interfaces/include/VectornavInterface.h | 4 +- lib/interfaces/include/VectornavInterface.tpp | 6 +- lib/mock_interfaces/VectornavInterface.h | 4 +- lib/shared_data/include/SharedDataTypes.h | 6 +- lib/systems/include/BaseLaunchController.h | 4 +- lib/systems/include/CASESystem.h | 70 +------------------ lib/systems/include/CASESystem.tpp | 2 +- lib/systems/include/LookupLaunchController.h | 2 +- lib/systems/include/SimpleLaunchController.h | 2 +- lib/systems/include/SlipLaunchController.h | 2 +- lib/systems/src/BaseLaunchController.cpp | 2 +- lib/systems/src/LaunchControllerAlgos.cpp | 6 +- src/main.cpp | 2 +- .../launch_controller_integration_testing.h | 2 +- 14 files changed, 23 insertions(+), 91 deletions(-) diff --git a/lib/interfaces/include/VectornavInterface.h b/lib/interfaces/include/VectornavInterface.h index 13435859a..d288e95bb 100644 --- a/lib/interfaces/include/VectornavInterface.h +++ b/lib/interfaces/include/VectornavInterface.h @@ -11,7 +11,7 @@ class VNInterface /* Watchdog last kicked time */ message_queue *msg_queue_; uint32_t can_id_; - vectornav vn_data; + VectornavData_s vn_data; public: VNInterface(message_queue *msg_output_queue) { @@ -33,7 +33,7 @@ class VNInterface void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg); void receive_ang_rates_CAN(CAN_message_t &recvd_msg); // getters - vectornav get_vn_struct(); + VectornavData_s get_vn_struct(); uint32_t get_id() { return can_id_;}; }; diff --git a/lib/interfaces/include/VectornavInterface.tpp b/lib/interfaces/include/VectornavInterface.tpp index aa0bc6a2c..3f081a077 100644 --- a/lib/interfaces/include/VectornavInterface.tpp +++ b/lib/interfaces/include/VectornavInterface.tpp @@ -74,7 +74,7 @@ template void VNInterface::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) { VN_ECEF_POS_Z_MSG_t ecef_z; Unpack_VN_ECEF_POS_Z_MSG_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); - vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_msg_ro_fromS(ecef_z.vn_ecef_pos_z_ro); + vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro); } template @@ -90,9 +90,9 @@ void VNInterface::receive_ang_rates_CAN(CAN_message_t & recvd_msg * @brief * getter method for returning vn_data structure * @tparam message_queue - * @return vectornav + * @return VectornavData_s */ template -vectornav VNInterface::get_vn_struct() { +VectornavData_s VNInterface::get_vn_struct() { return vn_data; } diff --git a/lib/mock_interfaces/VectornavInterface.h b/lib/mock_interfaces/VectornavInterface.h index 7c2289e19..c686e3ceb 100644 --- a/lib/mock_interfaces/VectornavInterface.h +++ b/lib/mock_interfaces/VectornavInterface.h @@ -10,7 +10,7 @@ class VNInterface /* Watchdog last kicked time */ message_queue *msg_queue_; uint32_t can_id_; - vectornav vn_data; + VectornavData_s vn_data; public: VNInterface(message_queue *msg_output_queue) { @@ -19,7 +19,7 @@ class VNInterface /* Kick watchdog */ // getters - vectornav get_vn_struct(); + VectornavData_s get_vn_struct(); uint32_t get_id() { return can_id_;}; }; diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index a40add941..6f791bea3 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -60,7 +60,7 @@ struct TorqueControllerOutput_s DrivetrainCommand_s command; bool ready; }; -struct vectornav +struct VectornavData_s { float velocity_x; float velocity_y; @@ -132,14 +132,14 @@ struct SharedCarState_s DrivetrainDynamicReport_s drivetrain_data; LoadCellInterfaceOutput_s loadcell_data; PedalsSystemData_s pedals_data; - vectornav vn_data; + VectornavData_s vn_data; SharedCarState_s() = delete; SharedCarState_s(const SysTick_s &_systick, const SteeringSystemData_s &_steering_data, const DrivetrainDynamicReport_s &_drivetrain_data, const LoadCellInterfaceOutput_s &_loadcell_data, const PedalsSystemData_s &_pedals_data, - const vectornav &_vn_data) + const VectornavData_s &_vn_data) : systick(_systick), steering_data(_steering_data), drivetrain_data(_drivetrain_data), diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 68f8dcfe5..108feeabf 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -47,9 +47,9 @@ class BaseLaunchController : public virtual Controller void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], - const vectornav &vn_data); + const VectornavData_s &vn_data); LaunchStates_e get_launch_state() { return launch_state_; } - virtual void calc_launch_algo(const vectornav &vn_data) = 0; + virtual void calc_launch_algo(const VectornavData_s &vn_data) = 0; 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 3349d9488..b1f4e1937 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -136,7 +136,7 @@ class CASESystem /// @return controller output DrivetrainCommand_s evaluate( const SysTick_s &tick, - const vectornav &vn_data, + const VectornavData_s &vn_data, const SteeringSystemData_s &steering_data, const DrivetrainDynamicReport_s &drivetrain_data, const veh_vec &load_cell_vals, @@ -146,80 +146,12 @@ class CASESystem bool start_button_pressed, uint8_t vn_status); - // void update_pid(float yaw_p, float yaw_i, float yaw_d, float tcs_p, float tcs_i, float tcs_d, float brake_p, float brake_i, float brake_d) - // { - // config_.yaw_pid_p = yaw_p; - // config_.yaw_pid_p = yaw_i; - // config_.yaw_pid_p = yaw_d; - - // config_.tcs_pid_p = tcs_p; - // config_.tcs_pid_i = tcs_i; - // config_.tcs_pid_d = tcs_d; - - // config_.yaw_pid_brakes_p = brake_p; - // config_.yaw_pid_brakes_i = brake_i; - // config_.yaw_pid_brakes_d = brake_d; - // } - DrivetrainCommand_s get_current_drive_command() { return current_command_; } float calculate_torque_request(const PedalsSystemData_s &pedals_data, 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 - // void update_config_from_param_interface(ParameterInterface ¶m_interface_ref) - // { - // config cfg = param_interface_ref.get_config(); - // config_.AbsoluteTorqueLimit = cfg.AbsoluteTorqueLimit; - // config_.yaw_pid_p = cfg.yaw_pid_p; - // config_.yaw_pid_i = cfg.yaw_pid_i; - // config_.yaw_pid_d = cfg.yaw_pid_d; - // config_.tcs_pid_p_lowerBound_front = cfg.tcs_pid_p_lowerBound_front; - // config_.tcs_pid_p_upperBound_front = cfg.tcs_pid_p_upperBound_front; - // config_.tcs_pid_p_lowerBound_rear = cfg.tcs_pid_p_lowerBound_rear; - // config_.tcs_pid_p_upperBound_rear = cfg.tcs_pid_p_upperBound_rear; - // config_.tcs_pid_i = cfg.tcs_pid_i; - // config_.tcs_pid_d = cfg.tcs_pid_d; - // config_.useLaunch = cfg.useLaunch; - // config_.usePIDTV = cfg.usePIDTV; - // config_.useTCSLimitedYawPID = cfg.useTCSLimitedYawPID; - // config_.useNormalForce = cfg.useNormalForce; - // config_.useTractionControl = cfg.useTractionControl; - // config_.usePowerLimit = cfg.usePowerLimit; - // config_.usePIDPowerLimit = cfg.usePIDPowerLimit; - // config_.useDecoupledYawBrakes = cfg.useDecoupledYawBrakes; - // config_.useDiscontinuousYawPIDBrakes = cfg.useDiscontinuousYawPIDBrakes; - // config_.tcsSLThreshold = cfg.tcsSLThreshold; - // config_.launchSL = cfg.launchSL; - // config_.launchDeadZone = cfg.launchDeadZone; - // config_.launchVelThreshold = cfg.launchVelThreshold; - // config_.tcsVelThreshold = cfg.tcsVelThreshold; - // config_.yawPIDMaxDifferential = cfg.yawPIDMaxDifferential; - // config_.yawPIDErrorThreshold = cfg.yawPIDErrorThreshold; - // config_.yawPIDVelThreshold = cfg.yawPIDVelThreshold; - // config_.yawPIDCoastThreshold = cfg.yawPIDCoastThreshold; - // config_.yaw_pid_brakes_p = cfg.yaw_pid_brakes_p; - // config_.yaw_pid_brakes_i = cfg.yaw_pid_brakes_i; - // config_.yaw_pid_brakes_d = cfg.yaw_pid_brakes_d; - // config_.decoupledYawPIDBrakesMaxDIfference = cfg.decoupledYawPIDBrakesMaxDIfference; - // config_.discontinuousBrakesPercentThreshold = cfg.discontinuousBrakesPercentThreshold; - // config_.TorqueMode = cfg.TorqueMode; - // config_.RegenLimit = cfg.RegenLimit; - // config_.useNoRegen5kph = cfg.useNoRegen5kph; - // config_.useTorqueBias = cfg.useTorqueBias; - // config_.DriveTorquePercentFront = cfg.DriveTorquePercentFront; - // config_.BrakeTorquePercentFront = cfg.BrakeTorquePercentFront; - // config_.MechPowerMaxkW = cfg.MechPowerMaxkW; - // config_.launchLeftRightMaxDiff = cfg.launchLeftRightMaxDiff; - // config_.tcs_pid_lower_rpm_front = cfg.tcs_pid_lower_rpm_front; - // config_.tcs_pid_upper_rpm_front = cfg.tcs_pid_upper_rpm_front; - // config_.tcs_pid_lower_rpm_rear = cfg.tcs_pid_lower_rpm_rear; - // config_.tcs_pid_upper_rpm_rear = cfg.tcs_pid_upper_rpm_rear; - // config_.maxNormalLoadBrakeScalingFront = cfg.maxNormalLoadBrakeScalingFront; - // config_.max_rpm = cfg.max_rpm; - // config_.max_regen_torque = cfg.max_regen_torque; - // config_.max_torque = cfg.max_torque; - // } 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 bd4f94695..33c7d7468 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -3,7 +3,7 @@ template DrivetrainCommand_s CASESystem::evaluate( const SysTick_s &tick, - const vectornav &vn_data, + const VectornavData_s &vn_data, const SteeringSystemData_s &steering_data, const DrivetrainDynamicReport_s &drivetrain_data, const veh_vec &load_cell_vals, diff --git a/lib/systems/include/LookupLaunchController.h b/lib/systems/include/LookupLaunchController.h index 6c0b80357..9072005fd 100644 --- a/lib/systems/include/LookupLaunchController.h +++ b/lib/systems/include/LookupLaunchController.h @@ -25,7 +25,7 @@ class TorqueControllerLookupLaunch : public virtual BaseLaunchController TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - void calc_launch_algo(const vectornav &vn_data) override; + void calc_launch_algo(const VectornavData_s &vn_data) override; }; diff --git a/lib/systems/include/SimpleLaunchController.h b/lib/systems/include/SimpleLaunchController.h index aa5be8bbf..ef4f84a64 100644 --- a/lib/systems/include/SimpleLaunchController.h +++ b/lib/systems/include/SimpleLaunchController.h @@ -31,6 +31,6 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - void calc_launch_algo(const vectornav &vn_data) override; + 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/SlipLaunchController.h b/lib/systems/include/SlipLaunchController.h index 0c80c6d55..f3b61eeb6 100644 --- a/lib/systems/include/SlipLaunchController.h +++ b/lib/systems/include/SlipLaunchController.h @@ -27,6 +27,6 @@ class TorqueControllerSlipLaunch : public virtual BaseLaunchController TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - void calc_launch_algo(const vectornav &vn_data) override; + void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/src/BaseLaunchController.cpp b/lib/systems/src/BaseLaunchController.cpp index cdbc6f6b2..c0bd646db 100644 --- a/lib/systems/src/BaseLaunchController.cpp +++ b/lib/systems/src/BaseLaunchController.cpp @@ -4,7 +4,7 @@ void BaseLaunchController::tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], - const vectornav &vn_data) + const VectornavData_s &vn_data) { if (tick.triggers.trigger100) diff --git a/lib/systems/src/LaunchControllerAlgos.cpp b/lib/systems/src/LaunchControllerAlgos.cpp index d5a107006..9995c359f 100644 --- a/lib/systems/src/LaunchControllerAlgos.cpp +++ b/lib/systems/src/LaunchControllerAlgos.cpp @@ -3,7 +3,7 @@ #include "SimpleLaunchController.h" -void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav &vn_data) +void TorqueControllerSimpleLaunch::calc_launch_algo(const VectornavData_s &vn_data) { /* Stolen launch algo from HT07. This ramps up the speed target over time. @@ -18,7 +18,7 @@ void TorqueControllerSimpleLaunch::calc_launch_algo(const vectornav &vn_data) launch_speed_target_ = std::min((int)PhysicalParameters::AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); } -void TorqueControllerLookupLaunch::calc_launch_algo(const vectornav &vn_data) +void TorqueControllerLookupLaunch::calc_launch_algo(const VectornavData_s &vn_data) { launch_speed_target_ = std::max((float)LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); @@ -44,7 +44,7 @@ void TorqueControllerLookupLaunch::calc_launch_algo(const vectornav &vn_data) float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); } -void TorqueControllerSlipLaunch::calc_launch_algo(const vectornav &vn_data) +void TorqueControllerSlipLaunch::calc_launch_algo(const VectornavData_s &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 diff --git a/src/main.cpp b/src/main.cpp index 342034de0..bc79ba105 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -664,7 +664,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) drivetrain.tick(current_system_tick); // // tick torque controller mux - DrivetrainCommand_s controller_output = case_system.evaluate( + auto _ = case_system.evaluate( current_system_tick, vn_interface.get_vn_struct(), steering_system.getSteeringSystemData(), diff --git a/test/test_systems/launch_controller_integration_testing.h b/test/test_systems/launch_controller_integration_testing.h index b7a52b6b5..3dafac6ae 100644 --- a/test/test_systems/launch_controller_integration_testing.h +++ b/test/test_systems/launch_controller_integration_testing.h @@ -112,7 +112,7 @@ TEST(LaunchIntergationTesting, test_slip_launch_controller) SysTick_s cur_tick; cur_tick = clock.tick(0); - vectornav vn_data{}; + VectornavData_s vn_data{}; // mode 0 TorqueControllerSimple tc_simple(1.0f, 1.0f); // mode 1 From c14698676e661a61ac041f176745d3b403e0031a Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 12:12:32 -0400 Subject: [PATCH 19/31] removing templated tc mux, added typedefinition that can be depended upon --- lib/state_machine/include/MCUStateMachine.h | 2 +- lib/state_machine/include/MCUStateMachine.tpp | 16 ++++---- lib/systems/include/TorqueControllerMux.h | 9 ++--- src/main.cpp | 6 +-- test/test_systems/state_machine_test.h | 40 +++++++++---------- 5 files changed, 34 insertions(+), 39 deletions(-) diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index db6707f21..d142d4bdb 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -23,7 +23,7 @@ enum class CAR_STATE READY_TO_DRIVE = 5 }; -template +template class MCUStateMachine { public: diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index 9e75321b1..36be0afa0 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -1,7 +1,7 @@ #include "MCUStateMachine.h" -template -void MCUStateMachine::tick_state_machine(unsigned long current_millis, const SharedCarState_s ¤t_car_state) +template +void MCUStateMachine::tick_state_machine(unsigned long current_millis, const SharedCarState_s ¤t_car_state) { switch (get_state()) { @@ -158,8 +158,8 @@ void MCUStateMachine::tick_state_machine(unsigned } } -template -void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) +template +void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) { // hal_println("running exit logic"); @@ -172,8 +172,8 @@ void MCUStateMachine::set_state_(CAR_STATE new_sta handle_entry_logic_(new_state, curr_time); } -template -void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_time) +template +void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state, unsigned long curr_time) { switch (get_state()) { @@ -195,8 +195,8 @@ void MCUStateMachine::handle_exit_logic_(CAR_STATE } } } -template -void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_time) +template +void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state, unsigned long curr_time) { switch (new_state) { diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index a298ed19b..1db0cfb80 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -53,11 +53,6 @@ namespace TC_MUX_DEFAULT_PARAMS constexpr const float MAX_POWER_LIMIT = 63000.0; }; - -// namespace TC_MUX -// { - - template class TorqueControllerMux { @@ -123,5 +118,9 @@ class TorqueControllerMux const SharedCarState_s &input_state); }; // } + +const int number_of_controllers = 5; +using TCMuxType = TorqueControllerMux; + #include "TorqueControllerMux.tpp" #endif // __TorqueControllerMux_H__ \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index bc79ba105..4b0574d28 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -306,9 +306,7 @@ TorqueControllerCASEWrapper case_wrapper(&case_system); TorqueControllerSimpleLaunch simple_launch; // mode 4 TorqueControllerSlipLaunch slip_launch; -const int number_of_controllers = 5; -TorqueControllerMux - torque_controller_mux({static_cast(&tc_simple), +TCMuxType torque_controller_mux({static_cast(&tc_simple), static_cast(&tc_vec), static_cast(&case_wrapper), static_cast(&simple_launch), @@ -316,7 +314,7 @@ TorqueControllerMux {false, false, true, false, false}); /* Declare state machine */ -MCUStateMachine> fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); +MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); // /* // GROUPING STRUCTS (To limit parameter count in utilizing functions) diff --git a/test/test_systems/state_machine_test.h b/test/test_systems/state_machine_test.h index 2e307d5e4..934e3a007 100644 --- a/test/test_systems/state_machine_test.h +++ b/test/test_systems/state_machine_test.h @@ -5,14 +5,11 @@ #include "MCUStateMachine.h" #include "fake_controller_type.h" -class FakeTCMux + +class DumbController : public Controller { -public: - DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, - TorqueLimit_e requested_torque_limit, - const SharedCarState_s &input_state) { return {}; } + TorqueControllerOutput_s evaluate(const SharedCarState_s & state) {return {};} }; - class DrivetrainMock { public: @@ -36,8 +33,9 @@ class DrivetrainMock }; SharedCarState_s dummy_state({}, {}, {}, {}, {}, {}); +DumbController c; -void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) +void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) { // ticking without hv over threshold testing and ensuring the tractive system not active still auto sys_time2 = sys_time; @@ -75,9 +73,9 @@ TEST(MCUStateMachineTesting, test_state_machine_init_tick) DrivetrainMock drivetrain; PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; EXPECT_EQ(state_machine.get_state(), CAR_STATE::STARTUP); state_machine.tick_state_machine(sys_time, dummy_state); @@ -92,9 +90,9 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true});; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; // ticking without hv over threshold testing and ensuring the tractive system not active still @@ -128,9 +126,9 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true});; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); // ticking without hv over threshold testing and ensuring the tractive system not active still state_machine.tick_state_machine(sys_time, dummy_state); @@ -174,9 +172,9 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); @@ -207,10 +205,10 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) DrivetrainMock drivetrain; PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true});; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; @@ -241,10 +239,10 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti drivetrain.drivetrain_error_ = false; PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true});; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; @@ -287,10 +285,10 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - FakeTCMux tc_mux; + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true});; SafetySystem ss(&ams, 0); - MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); + MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; From b72f25d1c08e5da9b006627e73caaea17b759bfa Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 17:41:15 -0400 Subject: [PATCH 20/31] removing debug prints --- lib/interfaces/src/DashboardInterface.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index 8ea8d8699..0ae10472f 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -30,12 +30,6 @@ void DashboardInterface::read(const CAN_message_t &can_msg) void DashboardInterface::update_torque_mode_(bool button_pressed) { // detect high-to-low transition - Serial.println("prev button state: "); - Serial.println(prev_button_pressed_state_); - Serial.println("button pressed: "); - Serial.println(button_pressed); - Serial.println("torque mode limit"); - Serial.println(static_cast(_data.torque_limit_mode)); if (prev_button_pressed_state_ == true && button_pressed == false) { From 0e3e3247e78c1af7e1fe6ea364cc3f788c17ae30 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 17:42:16 -0400 Subject: [PATCH 21/31] decimal fix base launch controller --- lib/systems/include/BaseLaunchController.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 108feeabf..0a3fbd4a4 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -34,7 +34,7 @@ class BaseLaunchController : public virtual Controller LaunchStates_e launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; uint32_t current_millis_; float launch_speed_target_ = 0.0; - int16_t init_speed_target_ = 0.0; + int16_t init_speed_target_ = 0; public: BaseLaunchController(int16_t initial_speed_target) From f1a8170af996ebe2c81bad863561f99dc9cba0bb Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 17:45:19 -0400 Subject: [PATCH 22/31] fixing function signature --- lib/systems/include/CASESystem.h | 2 +- lib/systems/include/CASESystem.tpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index b1f4e1937..5e5c962f2 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -148,7 +148,7 @@ class CASESystem DrivetrainCommand_s get_current_drive_command() { return current_command_; } - float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm); + 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 diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 33c7d7468..1787cbb12 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -18,7 +18,7 @@ DrivetrainCommand_s CASESystem::evaluate( // in. as defined in HT08_CASE.h, ExtU_HT08_CASE_T in.SteeringWheelAngleDeg = steering_data.angle; - in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_regen_torque, config_.max_rpm); + in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_torque, config_.max_regen_torque, config_.max_rpm); in.YawRaterads = vn_data.angular_rates.z; @@ -283,7 +283,7 @@ DrivetrainCommand_s CASESystem::evaluate( } template -float CASESystem::calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm) +float CASESystem::calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_torque, float max_regen_torque, float max_rpm) { // accelRequest goes between 1.0 and -1.0 float accelRequest = pedals_data.accelPercent - pedals_data.regenPercent; From 95fbcfd1351559e4d91fd0d243b198986581bcc5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sat, 14 Sep 2024 18:35:12 -0400 Subject: [PATCH 23/31] updating to lastest CAN lib, adding status for null pointer controller for tc mux and adding test for status --- lib/shared_data/include/SharedDataTypes.h | 3 ++- lib/systems/include/TorqueControllerMux.tpp | 13 +++++++------ platformio.ini | 2 +- test/test_systems/tc_mux_v2_testing.h | 13 +++++++++++++ 4 files changed, 23 insertions(+), 8 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 6f791bea3..ec44c8721 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -85,7 +85,8 @@ enum class TorqueControllerMuxError NO_ERROR = 0, ERROR_SPEED_DIFF_TOO_HIGH = 1, ERROR_TORQUE_DIFF_TOO_HIGH = 2, - ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS =3 + ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS =3, + ERROR_CONTROLLER_NULL_POINTER =4 }; struct TorqueControllerMuxStatus diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index 3253dbe28..60825ad71 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -21,6 +21,12 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C return empty_command; } + if( (!(controller_pointers_[current_controller_mode_index])) || (!controller_pointers_[req_controller_mode_index])) + { + current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER; + return empty_command; + } + current_output = controller_pointers_[current_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; @@ -30,7 +36,6 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C { TorqueControllerOutput_s proposed_output = controller_pointers_[req_controller_mode_index]->evaluate(input_state); TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); - // std::cout << "error state " << static_cast(error_state) << std::endl; if (error_state == TorqueControllerMuxError::NO_ERROR) { current_status_.current_controller_mode_ = requested_controller_type; @@ -42,13 +47,10 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C if (!mux_bypass_limits_[current_controller_mode_index]) { current_status_.current_torque_limit_enum = requested_torque_limit; - // std::cout << "output torques before regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); - // std::cout << "output torques after regen limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; 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]; current_output.command = apply_power_limit_(current_output.command, input_state.drivetrain_data, max_power_limit_, torque_limit_map_[requested_torque_limit]); - // std::cout << "output torques after power limit " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; current_output.command = apply_positive_speed_limit_(current_output.command); } else{ @@ -147,9 +149,8 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con net_power += abs(out.torqueSetpoints[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); } - // std::cout <<"net power "<< net_power < (power_limit)) + if (net_power > power_limit) { // std::cout <<"net power too big" < test({nullptr}, {true}); + SharedCarState_s 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.speeds_rpm[i], 0.0f); + } + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); +} #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file From 055792ef5044ceb0a0d806396487af1ad9e40a15 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Tue, 17 Sep 2024 19:54:53 -0400 Subject: [PATCH 24/31] current status with some testing --- lib/systems/include/TorqueControllerMux.tpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index 60825ad71..406ff17b7 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -52,10 +52,12 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C current_status_.current_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; } 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; } // 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; From 44d14e41658f28ef0919e689fbd01a93839aa1f0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 12:17:06 -0400 Subject: [PATCH 25/31] removing artifacts of parameter interface existing --- lib/mock_interfaces/ParameterInterface.h | 46 ------------------------ src/main.cpp | 2 -- 2 files changed, 48 deletions(-) delete mode 100644 lib/mock_interfaces/ParameterInterface.h diff --git a/lib/mock_interfaces/ParameterInterface.h b/lib/mock_interfaces/ParameterInterface.h deleted file mode 100644 index c6b0cac1c..000000000 --- a/lib/mock_interfaces/ParameterInterface.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef PARAMETERINTERFACE -#define PARAMETERINTERFACE -#include "MCUStateMachine.h" -#include "ht_eth.pb.h" -#include "default_config.h" - -// yes, i know this is a singleton. im prototyping rn. -// TODO review if I can just give this a pointer to an ethernet port -class ParameterInterface -{ -public: - ParameterInterface(): current_car_state_(CAR_STATE::STARTUP), params_need_sending_(false), config_(DEFAULT_CONFIG) {} - - void update_car_state(const CAR_STATE& state) - { - current_car_state_ = state; - } - void update_config(const config &config) - { - if(static_cast(current_car_state_) < 5 ){ - config_ = config; - } - - } - config get_config() - { - return config_; - } - void set_params_need_sending() - { - params_need_sending_ = true; - } - void reset_params_need_sending() - { - params_need_sending_ = false; - } - bool params_need_sending() { return params_need_sending_; } - -private: - CAR_STATE current_car_state_; - bool params_need_sending_ = false; - config config_; - -}; - -#endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 4b0574d28..de3ecf678 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,6 @@ /* Include files */ /* System Includes*/ #include -// #include "ParameterInterface.h" /* Libraries */ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" @@ -32,7 +31,6 @@ #include "SafetySystem.h" #include "DrivetrainSystem.h" #include "PedalsSystem.h" -// #include "TorqueControllerMux.h" #include "TorqueControllerMux.h" #include "CASESystem.h" From babe696c8e55b1af458a88b184d46e079cfef16f Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 12:19:33 -0400 Subject: [PATCH 26/31] fixing formatting and removing commented includes --- src/main.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index de3ecf678..2bef8eaa6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,7 +5,6 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" -// #include "NativeEthernet.h" // /* Interfaces */ @@ -23,7 +22,6 @@ #include "SABInterface.h" #include "VectornavInterface.h" #include "LoadCellInterface.h" -#include "TorqueControllers.h" /* Systems */ #include "SysClock.h" @@ -32,8 +30,9 @@ #include "DrivetrainSystem.h" #include "PedalsSystem.h" #include "TorqueControllerMux.h" - +#include "TorqueControllers.h" #include "CASESystem.h" + // /* State machine */ #include "MCUStateMachine.h" #include "HT08_CASE.h" From 7d44f8a5c64f087815fa3d92369adaea09c2c9b4 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 12:20:09 -0400 Subject: [PATCH 27/31] moving around include --- src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 2bef8eaa6..c73786ed5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,7 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" +#include "PrintLogger.h" // /* Interfaces */ @@ -37,7 +38,6 @@ #include "MCUStateMachine.h" #include "HT08_CASE.h" -#include "PrintLogger.h" /* PARAMETER STRUCTS */ From 36ded31993fdf0c05f102bfef8e01b244211595c Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 14:37:17 -0400 Subject: [PATCH 28/31] moving controllers into separate include directory --- .../{ => Controllers}/CASEControllerWrapper.h | 0 .../{ => Controllers}/LoadCellVectoringController.h | 0 .../{ => Controllers}/LookupLaunchController.h | 0 .../include/{ => Controllers}/SimpleController.h | 0 .../{ => Controllers}/SimpleLaunchController.h | 0 .../include/{ => Controllers}/SlipLaunchController.h | 0 lib/systems/include/TorqueControllers.h | 12 ++++++------ lib/systems/src/LaunchControllerAlgos.cpp | 6 +++--- lib/systems/src/LoadCellVectoringController.cpp | 2 +- lib/systems/src/SimpleController.cpp | 2 +- 10 files changed, 11 insertions(+), 11 deletions(-) rename lib/systems/include/{ => Controllers}/CASEControllerWrapper.h (100%) rename lib/systems/include/{ => Controllers}/LoadCellVectoringController.h (100%) rename lib/systems/include/{ => Controllers}/LookupLaunchController.h (100%) rename lib/systems/include/{ => Controllers}/SimpleController.h (100%) rename lib/systems/include/{ => Controllers}/SimpleLaunchController.h (100%) rename lib/systems/include/{ => Controllers}/SlipLaunchController.h (100%) diff --git a/lib/systems/include/CASEControllerWrapper.h b/lib/systems/include/Controllers/CASEControllerWrapper.h similarity index 100% rename from lib/systems/include/CASEControllerWrapper.h rename to lib/systems/include/Controllers/CASEControllerWrapper.h diff --git a/lib/systems/include/LoadCellVectoringController.h b/lib/systems/include/Controllers/LoadCellVectoringController.h similarity index 100% rename from lib/systems/include/LoadCellVectoringController.h rename to lib/systems/include/Controllers/LoadCellVectoringController.h diff --git a/lib/systems/include/LookupLaunchController.h b/lib/systems/include/Controllers/LookupLaunchController.h similarity index 100% rename from lib/systems/include/LookupLaunchController.h rename to lib/systems/include/Controllers/LookupLaunchController.h diff --git a/lib/systems/include/SimpleController.h b/lib/systems/include/Controllers/SimpleController.h similarity index 100% rename from lib/systems/include/SimpleController.h rename to lib/systems/include/Controllers/SimpleController.h diff --git a/lib/systems/include/SimpleLaunchController.h b/lib/systems/include/Controllers/SimpleLaunchController.h similarity index 100% rename from lib/systems/include/SimpleLaunchController.h rename to lib/systems/include/Controllers/SimpleLaunchController.h diff --git a/lib/systems/include/SlipLaunchController.h b/lib/systems/include/Controllers/SlipLaunchController.h similarity index 100% rename from lib/systems/include/SlipLaunchController.h rename to lib/systems/include/Controllers/SlipLaunchController.h diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 1a3e21b85..cbafecffa 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -1,7 +1,7 @@ //controllers -#include "CASEControllerWrapper.h" -#include "LoadCellVectoringController.h" -#include "LookupLaunchController.h" -#include "SimpleLaunchController.h" -#include "SimpleController.h" -#include "SlipLaunchController.h" +#include "Controllers/CASEControllerWrapper.h" +#include "Controllers/LoadCellVectoringController.h" +#include "Controllers/LookupLaunchController.h" +#include "Controllers/SimpleLaunchController.h" +#include "Controllers/SimpleController.h" +#include "Controllers/SlipLaunchController.h" diff --git a/lib/systems/src/LaunchControllerAlgos.cpp b/lib/systems/src/LaunchControllerAlgos.cpp index 9995c359f..dbc559e73 100644 --- a/lib/systems/src/LaunchControllerAlgos.cpp +++ b/lib/systems/src/LaunchControllerAlgos.cpp @@ -1,6 +1,6 @@ -#include "LookupLaunchController.h" -#include "SlipLaunchController.h" -#include "SimpleLaunchController.h" +#include "Controllers/LookupLaunchController.h" +#include "Controllers/SlipLaunchController.h" +#include "Controllers/SimpleLaunchController.h" void TorqueControllerSimpleLaunch::calc_launch_algo(const VectornavData_s &vn_data) diff --git a/lib/systems/src/LoadCellVectoringController.cpp b/lib/systems/src/LoadCellVectoringController.cpp index f6d32c644..e85502e84 100644 --- a/lib/systems/src/LoadCellVectoringController.cpp +++ b/lib/systems/src/LoadCellVectoringController.cpp @@ -1,4 +1,4 @@ -#include "LoadCellVectoringController.h" +#include "Controllers/LoadCellVectoringController.h" void TorqueControllerLoadCellVectoring::tick( diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/SimpleController.cpp index 08e308680..4906e4ed8 100644 --- a/lib/systems/src/SimpleController.cpp +++ b/lib/systems/src/SimpleController.cpp @@ -1,4 +1,4 @@ -#include "SimpleController.h" +#include "Controllers/SimpleController.h" void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) { From a8b3e3fffd86bc9e85947081dbca3db06b1855e0 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 14:43:34 -0400 Subject: [PATCH 29/31] moving controller implementations into separate directory --- lib/systems/src/{ => Controllers}/BaseLaunchController.cpp | 0 lib/systems/src/{ => Controllers}/LaunchControllerAlgos.cpp | 0 lib/systems/src/{ => Controllers}/LoadCellVectoringController.cpp | 0 lib/systems/src/{ => Controllers}/SimpleController.cpp | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename lib/systems/src/{ => Controllers}/BaseLaunchController.cpp (100%) rename lib/systems/src/{ => Controllers}/LaunchControllerAlgos.cpp (100%) rename lib/systems/src/{ => Controllers}/LoadCellVectoringController.cpp (100%) rename lib/systems/src/{ => Controllers}/SimpleController.cpp (100%) diff --git a/lib/systems/src/BaseLaunchController.cpp b/lib/systems/src/Controllers/BaseLaunchController.cpp similarity index 100% rename from lib/systems/src/BaseLaunchController.cpp rename to lib/systems/src/Controllers/BaseLaunchController.cpp diff --git a/lib/systems/src/LaunchControllerAlgos.cpp b/lib/systems/src/Controllers/LaunchControllerAlgos.cpp similarity index 100% rename from lib/systems/src/LaunchControllerAlgos.cpp rename to lib/systems/src/Controllers/LaunchControllerAlgos.cpp diff --git a/lib/systems/src/LoadCellVectoringController.cpp b/lib/systems/src/Controllers/LoadCellVectoringController.cpp similarity index 100% rename from lib/systems/src/LoadCellVectoringController.cpp rename to lib/systems/src/Controllers/LoadCellVectoringController.cpp diff --git a/lib/systems/src/SimpleController.cpp b/lib/systems/src/Controllers/SimpleController.cpp similarity index 100% rename from lib/systems/src/SimpleController.cpp rename to lib/systems/src/Controllers/SimpleController.cpp From bf19e97446efc981f11287c65fbe60c8f1d4e6a2 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 15:17:47 -0400 Subject: [PATCH 30/31] migrating MCU interface to use shared data type for tc mux status --- lib/interfaces/include/MCUInterface.h | 5 ++--- lib/interfaces/src/MCUInterface.cpp | 10 ++++++--- lib/shared_data/include/SharedDataTypes.h | 2 +- lib/systems/include/TorqueControllerMux.tpp | 6 ++--- src/main.cpp | 25 +++++---------------- test/test_systems/tc_mux_v2_testing.h | 8 +++---- 6 files changed, 22 insertions(+), 34 deletions(-) diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index 521f10301..d042ece3b 100644 --- a/lib/interfaces/include/MCUInterface.h +++ b/lib/interfaces/include/MCUInterface.h @@ -7,6 +7,7 @@ #include "hytech.h" #include "MessageQueueDefine.h" #include "PedalsSystem.h" +#include "SharedDataTypes.h" const int DEFAULT_BMS_OK_READ = 17; // SHDN_D_READ const int DEFAULT_BMS_SENSE_PIN = 16; // BMS_OK_SENSE @@ -123,9 +124,7 @@ class MCUInterface int fsm_state, bool inv_has_error, bool software_is_ok, - int drive_mode, - int torque_mode, - float max_torque, + const TorqueControllerMuxStatus& tc_mux_status, bool buzzer_is_on, const PedalsSystemData_s &pedals_data, bool pack_charge_is_critical, diff --git a/lib/interfaces/src/MCUInterface.cpp b/lib/interfaces/src/MCUInterface.cpp index ecd489e6f..b66b356c1 100644 --- a/lib/interfaces/src/MCUInterface.cpp +++ b/lib/interfaces/src/MCUInterface.cpp @@ -186,9 +186,8 @@ void MCUInterface::update_mcu_status_CAN_pedals(const PedalsSystemData_s &pedals void MCUInterface::tick(int fsm_state, bool inv_has_error, bool software_is_ok, - int drive_mode, - int torque_mode, - float max_torque, + + const TorqueControllerMuxStatus& tc_mux_status, bool buzzer_is_on, const PedalsSystemData_s &pedals_data, bool pack_charge_is_critical, @@ -199,6 +198,11 @@ void MCUInterface::tick(int fsm_state, // Systems 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; + update_mcu_status_CAN_TCMux(drive_mode, torque_mode, max_torque); update_mcu_status_CAN_buzzer(buzzer_is_on); update_mcu_status_CAN_pedals(pedals_data); diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index ec44c8721..2e399a6df 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -92,7 +92,7 @@ enum class TorqueControllerMuxError struct TorqueControllerMuxStatus { TorqueControllerMuxError current_error; - ControllerMode_e current_controller_mode_; + ControllerMode_e current_controller_mode; TorqueLimit_e current_torque_limit_enum; float current_torque_limit_value; bool output_is_bypassing_limits; diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index 406ff17b7..c392240bd 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -13,7 +13,7 @@ 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 current_controller_mode_index = static_cast(current_status_.current_controller_mode); if ((std::size_t)req_controller_mode_index > ( controller_pointers_.size() - 1 )) { @@ -30,7 +30,7 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C current_output = controller_pointers_[current_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_; + bool requesting_controller_change = requested_controller_type != current_status_.current_controller_mode; if (requesting_controller_change) { @@ -38,7 +38,7 @@ 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_status_.current_controller_mode = requested_controller_type; current_controller_mode_index = req_controller_mode_index; current_output = proposed_output; } diff --git a/src/main.cpp b/src/main.cpp index c73786ed5..a8c2fe435 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -332,7 +332,6 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); -void handle_ethernet_interface_comms(); /* SETUP @@ -407,8 +406,6 @@ void loop() // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); - // handle_ethernet_interface_comms(); - // process received CAN messages process_ring_buffer(CAN2_rxBuffer, CAN_receive_interfaces, curr_tick.millis); process_ring_buffer(CAN3_rxBuffer, CAN_receive_interfaces, curr_tick.millis); @@ -429,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().current_controller_mode), curr_tick.millis, 100); // inverter procedure before entering state machine // reset inverters if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) @@ -484,7 +481,7 @@ 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().current_controller_mode)); Serial.print("Current TC error: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); Serial.println(); @@ -536,16 +533,14 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) torque_controller_mux.get_tc_mux_status().current_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().current_controller_mode), dashboard.getDialMode()); main_ecu.tick( static_cast(fsm.get_state()), drivetrain.drivetrain_error_occured(), safety_system.get_software_is_ok(), - static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode_), - static_cast(torque_controller_mux.get_tc_mux_status().current_torque_limit_enum), - torque_controller_mux.get_tc_mux_status().current_torque_limit_value, + torque_controller_mux.get_tc_mux_status(), buzzer.buzzer_is_on(), pedals_system.getPedalsSystemData(), ams_interface.pack_charge_is_critical(), @@ -659,7 +654,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) drivetrain.tick(current_system_tick); // // tick torque controller mux - auto _ = case_system.evaluate( + auto __attribute__((unused)) case_status = case_system.evaluate( current_system_tick, vn_interface.get_vn_struct(), steering_system.getSteeringSystemData(), @@ -672,13 +667,3 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) vn_interface.get_vn_struct().vn_status); } - -void handle_ethernet_interface_comms() -{ - // function that will handle receiving and distributing of all messages to all ethernet interfaces - // via the union message. this is a little bit cursed ngl. - // TODO un fuck this and make it more sane - // Serial.println("bruh"); - // handle_ethernet_socket_receive(&protobuf_recv_socket, &recv_pb_stream_union_msg, ethernet_interfaces); - -} \ No newline at end of file diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 0d51b17f5..935566fe1 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -84,7 +84,7 @@ 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_controller_mode, ControllerMode_e::MODE_0); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH); set_outputs(inst1, 0, 1); @@ -93,7 +93,7 @@ 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_controller_mode, ControllerMode_e::MODE_1); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); } @@ -107,7 +107,7 @@ 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_controller_mode, ControllerMode_e::MODE_0); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); // tick it a bunch of times @@ -120,7 +120,7 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) ASSERT_EQ(test.get_tc_mux_status().current_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().current_controller_mode, ControllerMode_e::MODE_0); ASSERT_EQ(out1.torqueSetpoints[0], 1); ASSERT_EQ(out1.torqueSetpoints[1], 1); From 2277f4cc69f2022481bb80b0d309f487a9b95231 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Wed, 18 Sep 2024 15:19:42 -0400 Subject: [PATCH 31/31] renaming fake data type --- test/test_systems/fake_controller_type.h | 2 +- .../launch_controller_integration_testing.h | 12 ++++++------ test/test_systems/tc_mux_v2_testing.h | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/test/test_systems/fake_controller_type.h b/test/test_systems/fake_controller_type.h index becdb85ab..d27fa367d 100644 --- a/test/test_systems/fake_controller_type.h +++ b/test/test_systems/fake_controller_type.h @@ -1,7 +1,7 @@ #ifndef __FAKE_CONTROLLER_TYPE_H__ #define __FAKE_CONTROLLER_TYPE_H__ #include "BaseController.h" -struct dummy_queue +struct DummyQueue_s { }; diff --git a/test/test_systems/launch_controller_integration_testing.h b/test/test_systems/launch_controller_integration_testing.h index 3dafac6ae..2004ead69 100644 --- a/test/test_systems/launch_controller_integration_testing.h +++ b/test/test_systems/launch_controller_integration_testing.h @@ -60,9 +60,9 @@ TEST(LaunchIntergationTesting, test_simple_launch_controller) // mode 1 TorqueControllerLoadCellVectoring tc_vec; // mode 2 - dummy_queue q; - CASESystem case_sys(&q, 100, 70, 550, {}); - TorqueControllerCASEWrapper case_wrapper(&case_sys); + DummyQueue_s q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); // mode 3 TorqueControllerSimpleLaunch simple_launch; @@ -118,9 +118,9 @@ TEST(LaunchIntergationTesting, test_slip_launch_controller) // mode 1 TorqueControllerLoadCellVectoring tc_vec; // mode 2 - dummy_queue q; - CASESystem case_sys(&q, 100, 70, 550, {}); - TorqueControllerCASEWrapper case_wrapper(&case_sys); + DummyQueue_s q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); // mode 3 TorqueControllerSimpleLaunch simple_launch; diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 935566fe1..7d7d1beae 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -135,9 +135,9 @@ TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) // mode 1 TorqueControllerLoadCellVectoring tc_vec; // mode 2 - dummy_queue q; - CASESystem case_sys(&q, 100, 70, 550, {}); - TorqueControllerCASEWrapper case_wrapper(&case_sys); + DummyQueue_s q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); // mode 3 TorqueControllerSimpleLaunch simple_launch; @@ -162,9 +162,9 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) // mode 1 TorqueControllerLoadCellVectoring tc_vec; // mode 2 - dummy_queue q; - CASESystem case_sys(&q, 100, 70, 550, {}); - TorqueControllerCASEWrapper case_wrapper(&case_sys); + DummyQueue_s q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); // mode 3 TorqueControllerSimpleLaunch simple_launch;