From 055792ef5044ceb0a0d806396487af1ad9e40a15 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Tue, 17 Sep 2024 19:54:53 -0400 Subject: [PATCH 01/28] 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 02/28] 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 03/28] 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 04/28] 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 05/28] 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 06/28] 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 07/28] 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 08/28] 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; From 25f12f0fed6a9fb53f3a9e180f62728dfdc6fd74 Mon Sep 17 00:00:00 2001 From: Justin Hwang Date: Fri, 20 Sep 2024 00:12:28 -0400 Subject: [PATCH 09/28] Added brake sensor --- lib/interfaces/include/MCUInterface.h | 28 +++++++++++++++------------ lib/interfaces/src/MCUInterface.cpp | 14 ++++++++++++++ platformio.ini | 4 ++-- 3 files changed, 32 insertions(+), 14 deletions(-) diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index d042ece3b..e53110c5b 100644 --- a/lib/interfaces/include/MCUInterface.h +++ b/lib/interfaces/include/MCUInterface.h @@ -9,17 +9,18 @@ #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 -const int DEFAULT_IMD_OK_READ = 10; // SHDN_C_READ -const int DEFAULT_IMD_SENSE_PIN = 18; // OKHS_SENSE -const int DEFAULT_BSPD_OK_READ = 39; // SHDN_E_READ -const int DEFAULT_SOFTWARE_OK_READ = 25; // SHDN_F_READ Watchdog Combined -const int DEFAULT_BOTS_OK_READ = 24; // SHDN_B_READ -const int DEFAULT_BRB_OK_READ = 26; // SHDN_G_READ -const int DEFAULT_BRAKE_LIGHT_CTRL = 6; -const int DEFAULT_INVERTER_ENABLE = 9; -const int DEFAULT_INVERTER_24V_ENABLE = 7; +const int DEFAULT_BMS_OK_READ = 17; // SHDN_D_READ +const int DEFAULT_BMS_SENSE_PIN = 16; // BMS_OK_SENSE +const int DEFAULT_IMD_OK_READ = 10; // SHDN_C_READ +const int DEFAULT_IMD_SENSE_PIN = 18; // OKHS_SENSE +const int DEFAULT_BSPD_OK_READ = 39; // SHDN_E_READ +const int DEFAULT_SOFTWARE_OK_READ = 25; // SHDN_F_READ Watchdog Combined +const int DEFAULT_BOTS_OK_READ = 24; // SHDN_B_READ +const int DEFAULT_BRB_OK_READ = 26; // SHDN_G_READ +const int DEFAULT_BRAKE_LIGHT_CTRL = 6; +const int DEFAULT_INVERTER_ENABLE = 9; +const int DEFAULT_INVERTER_24V_ENABLE = 7; +const int DEFAULT_BRAKE_PRESSURE_SENSOR_READ = 27; // Read pin for brake pressure sensor. /// @brief specifically designed so that Walker would be happy struct MainECUHardwareReadPins @@ -36,10 +37,12 @@ struct MainECUHardwareReadPins // inverter enable pins int pin_inv_en; int pin_inv_24V_en; + int pin_brake_pressure_sensor_read; }; static const MainECUHardwareReadPins DEFAULT_PINS = {DEFAULT_BMS_OK_READ,DEFAULT_IMD_OK_READ, DEFAULT_BSPD_OK_READ, DEFAULT_SOFTWARE_OK_READ, - DEFAULT_BOTS_OK_READ, DEFAULT_BRB_OK_READ, DEFAULT_BRAKE_LIGHT_CTRL, DEFAULT_INVERTER_ENABLE, DEFAULT_INVERTER_24V_ENABLE}; + DEFAULT_BOTS_OK_READ, DEFAULT_BRB_OK_READ, DEFAULT_BRAKE_LIGHT_CTRL, DEFAULT_INVERTER_ENABLE, + DEFAULT_INVERTER_24V_ENABLE, DEFAULT_BRAKE_PRESSURE_SENSOR_READ}; class MCUInterface { @@ -115,6 +118,7 @@ class MCUInterface // Interfaces void update_mcu_status_CAN_ams(bool is_critical); void update_mcu_status_CAN_dashboard(bool is_active); + void update_brake_pressure_CAN(); /* Enqueue MCU_status CAN */ void enqueue_CAN_mcu_status(); diff --git a/lib/interfaces/src/MCUInterface.cpp b/lib/interfaces/src/MCUInterface.cpp index b66b356c1..dabb3bc37 100644 --- a/lib/interfaces/src/MCUInterface.cpp +++ b/lib/interfaces/src/MCUInterface.cpp @@ -182,6 +182,19 @@ void MCUInterface::update_mcu_status_CAN_pedals(const PedalsSystemData_s &pedals mcu_status_.no_brake_implausibility = !pedals.brakeImplausible; mcu_status_.no_accel_or_brake_implausibility = !(pedals.brakeAndAccelPressedImplausibility); } +void MCUInterface::update_brake_pressure_CAN() +{ + BRAKE_PRESSURE_SENSOR_t brake_sensor_msg; + brake_sensor_msg.brake_sensor_analog_read = analogRead(pins_.pin_brake_pressure_sensor_read); + + CAN_message_t msg; + + msg.id = Pack_BRAKE_PRESSURE_SENSOR_hytech(&brake_sensor_msg, msg.buf, &msg.len, (uint8_t*) &msg.flags.extended); + + uint8_t buf[sizeof(CAN_message_t)] = {}; + memmove(buf, &msg, sizeof(CAN_message_t)); + msg_queue_->push_back(buf, sizeof(CAN_message_t)); +} void MCUInterface::tick(int fsm_state, bool inv_has_error, @@ -209,6 +222,7 @@ void MCUInterface::tick(int fsm_state, // External Interfaces update_mcu_status_CAN_ams(pack_charge_is_critical); update_mcu_status_CAN_dashboard(button_is_pressed); + update_brake_pressure_CAN(); // Internal values update_mcu_status_CAN(); // Push into buffer diff --git a/platformio.ini b/platformio.ini index a9474fcd6..7ff412f6f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -58,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/125/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz [env:test_can_on_teensy] lib_ignore = @@ -81,7 +81,7 @@ lib_deps = https://github.com/hytech-racing/shared_firmware_interfaces.git#feature/thermistor-template https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 https://github.com/RCMast3r/spi_libs#2214fee - https://github.com/hytech-racing/HT_CAN/releases/download/108/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/127/can_lib.tar.gz git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 From ad8c202e763ce08f79baaa7edaafbcc1089cdd69 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sat, 21 Sep 2024 14:52:07 -0400 Subject: [PATCH 10/28] simple tests and outline of testing structure --- lib/systems/include/TorqueControllerMux.h | 1 - test/test_systems/tc_mux_v2_testing.h | 336 ++++++++++++++++------ 2 files changed, 249 insertions(+), 88 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 1db0cfb80..21d89efb2 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -75,7 +75,6 @@ class TorqueControllerMux 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); diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 7d7d1beae..8b8e37308 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -4,6 +4,7 @@ #include "TorqueControllerMux.h" #include "TorqueControllers.h" #include "fake_controller_type.h" +#include @@ -42,29 +43,73 @@ void set_output_rpm(controller_type &controller, float rpm, float torque) controller.output.command.torqueSetpoints[2] = torque; controller.output.command.torqueSetpoints[3] = torque; } + +//construction testing TEST(TorqueControllerMuxTesting, test_construction) { TestControllerType inst1, inst2; TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); } - -TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) +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, false, true, false, false}); +} +TEST(TorqueControllerMuxTesting, test_construction_bypass_limits) { TestControllerType inst1, inst2; - TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - 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); - for (int i =0; i< 4; i++) + set_output_rpm(inst1, 500, 30.0); + set_output_rpm(inst2, 500, 30.0); + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {true, false}); + + DrivetrainDynamicReport_s drivetrain_data = {}; + for (int i = 0; i < 4; i++) { - - ASSERT_EQ(res.speeds_rpm[i], 0.0); - ASSERT_EQ(res.torqueSetpoints[i], 0.0); + drivetrain_data.measuredSpeeds[i] = 500.0f; } + //onlhy use mode 0 for the input state in all tests and then test if they would act the same, idk how to do that + 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); + + ASSERT_EQ(drive_command.torqueSetpoints[0], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[1], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[2], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[3], 30.0f); + + ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true); + + inst1.output.command.torqueSetpoints[0] = 5; + 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], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[2], 30.0f); + ASSERT_EQ(drive_command.torqueSetpoints[3], 30.0f); + + ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true); } - - + +//logic testing // 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) @@ -75,28 +120,82 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); SharedCarState_s 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); - + // 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); 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_controller_mode_, ControllerMode_e::MODE_1); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); } - +TEST(TorqueControllerMuxTesting, simple_switch_many_active_modes) +{ + TestControllerType inst1, inst2, inst3, inst4; + set_outputs(inst1, 0, 1); + set_outputs(inst2, 0, 1); + set_outputs(inst3, 0, 1); + set_outputs(inst4, 0, 1); + TorqueControllerMux<4> test({static_cast(&inst1), static_cast(&inst2), static_cast(&inst3), static_cast(&inst4)}, {false, false, false, false}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); + set_four_outputs(state.drivetrain_data.measuredSpeeds, 2); + + state.pedals_data = {}; + state.vn_data = {}; + + auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, 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::NO_ERROR); + + 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); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_3); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, 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::NO_ERROR); +} + +//TorqueControllerMuxError testing +TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) +{ + TestControllerType inst1, inst2; + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + 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); + for (int i =0; i< 4; i++) + { + + ASSERT_EQ(res.speeds_rpm[i], 0.0); + ASSERT_EQ(res.torqueSetpoints[i], 0.0); + } +} TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) { TestControllerType inst1, inst2; @@ -104,12 +203,12 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) set_outputs(inst2, 3, 10); TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); 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); 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); @@ -117,55 +216,45 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); 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(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) +TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) { - // mode 0 - TorqueControllerSimple tc_simple(1.0f, 1.0f); - // mode 1 - TorqueControllerLoadCellVectoring tc_vec; - // mode 2 - DummyQueue_s 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}); + TorqueControllerMux<1> 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); + } + //makes it not able to test, assume i didnt pull most recent code + // ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); } - + +//make generic +//mode evalutaion testing TEST(TorqueControllerMuxTesting, test_mode0_evaluation) { - + float max_torque = 21.42; // mode 0 TorqueControllerSimple tc_simple(1.0f, 1.0f); // mode 1 TorqueControllerLoadCellVectoring tc_vec; // mode 2 - DummyQueue_s q; - CASESystem case_sys(&q, 100, 70, 550, {}); - TorqueControllerCASEWrapper case_wrapper(&case_sys); - + dummy_queue q; + CASESystem case_sys(&q, 100, 70, 550, {}); + TorqueControllerCASEWrapper case_wrapper(&case_sys); + // mode 3 TorqueControllerSimpleLaunch simple_launch; // mode 4 @@ -177,38 +266,39 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) static_cast(&slip_launch)}, {false, true, false, false, false}); SharedCarState_s mode_0_input_state({}, {}, {}, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); - + DrivetrainCommand_s out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); ASSERT_NEAR(out.torqueSetpoints[0], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[1], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[2], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[3], (max_torque / 2), 0.01); - + 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); } - + +//limit testing 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; } 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); - + for (int i = 0; i < 4; i++) { ASSERT_EQ(res.torqueSetpoints[i], 10.0f); @@ -218,66 +308,138 @@ TEST(TorqueControllerMuxTesting, test_power_limit) drivetrain_data.measuredSpeeds[i] = 20000.0f; } set_output_rpm(inst1, 20000, 21.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++) { 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; } - + 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); - + 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]); } -TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) +//integration testing +TEST(TorqueControllerMuxTesting, test_tc_mux_status) { - TorqueControllerMux<1> test({nullptr}, {true}); + TorqueControllerMuxError testErr = TorqueControllerMuxError::NO_ERROR; + ControllerMode_e testMode = ControllerMode_e::MODE_0; + TorqueLimit_e testTorqLim = TorqueLimit_e::TCMUX_FULL_TORQUE; + TestControllerType inst1; + set_outputs(inst1, 0, 1); + TorqueControllerMux<1> test({static_cast(&inst1)}, { false }); 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); + set_four_outputs(state.drivetrain_data.measuredSpeeds, 2); + + state.pedals_data = {}; + state.vn_data = {}; + + auto out1 = test.getDrivetrainCommand(testMode, testTorqLim, state); + ASSERT_EQ(test.get_tc_mux_status().current_error, testErr); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, testMode); + ASSERT_EQ(test.get_tc_mux_status().current_torque_limit_enum, testTorqLim); } +TEST(TorqueControllerMuxTesting, test_real_controllers_output) +{ + //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, false, false, false}); + SharedCarState_s state({}, {}, {}, {}, {}, {}); + state.pedals_data = {}; + state.vn_data = {}; + state.drivetrain_data = {}; + + auto out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_0); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_1); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(static_cast(torque_controller_mux.get_tc_mux_status().current_error), static_cast(TorqueControllerMuxError::NO_ERROR)); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_3); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_4); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_4); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_3); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + + out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_0); +} + #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file From 11ed4ea18da301c993e2be2b094c438052fd33ff Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sat, 21 Sep 2024 15:20:01 -0400 Subject: [PATCH 11/28] updated working rebase --- test/test_systems/tc_mux_v2_testing.h | 56 +++++++++++++-------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 8b8e37308..086fc108b 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -57,9 +57,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; @@ -129,7 +129,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); @@ -138,7 +138,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); } TEST(TorqueControllerMuxTesting, simple_switch_many_active_modes) @@ -156,27 +156,27 @@ TEST(TorqueControllerMuxTesting, simple_switch_many_active_modes) state.vn_data = {}; auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, 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::NO_ERROR); 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); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_3); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, 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::NO_ERROR); } @@ -219,7 +219,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); @@ -251,9 +251,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; @@ -374,7 +374,7 @@ TEST(TorqueControllerMuxTesting, test_tc_mux_status) auto out1 = test.getDrivetrainCommand(testMode, testTorqLim, state); ASSERT_EQ(test.get_tc_mux_status().current_error, testErr); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode_, testMode); + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, testMode); ASSERT_EQ(test.get_tc_mux_status().current_torque_limit_enum, testTorqLim); } TEST(TorqueControllerMuxTesting, test_real_controllers_output) @@ -384,9 +384,9 @@ TEST(TorqueControllerMuxTesting, test_real_controllers_output) // 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; @@ -407,39 +407,39 @@ TEST(TorqueControllerMuxTesting, test_real_controllers_output) auto out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_0); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_1); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_1); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(static_cast(torque_controller_mux.get_tc_mux_status().current_error), static_cast(TorqueControllerMuxError::NO_ERROR)); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_3); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_4); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_4); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_4); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_4); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_3); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_2); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode_, ControllerMode_e::MODE_0); + ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); } #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file From 487d15dda3ab62636364d122fb81ebb4a8e5b8d0 Mon Sep 17 00:00:00 2001 From: Miles Kent Date: Sat, 21 Sep 2024 16:09:23 -0400 Subject: [PATCH 12/28] Docs for DrivetrainCommand_s --- lib/systems/include/TorqueControllerMux.h | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 1db0cfb80..b8d0b7bfc 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -76,9 +76,24 @@ class TorqueControllerMux DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out); + /// @brief Clamps negative rpms to 0f + /// @param const DrivetrainCommand_s &command provides the rpm info as a DrivetrainCommand_s DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); + + /// @brief Ensure torque is at most at the specified limit. If exceeding, then limit it in the returned DrivetrainCommand_s + /// @param const DrivetrainCommand_s &command is a DrivetrainCommand_s, which provides torque info + /// @param float max_torque is the maximum average torque the wheels are allowed to experience before it is limited. DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); + + /// @brief 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 + /// @param const DrivetrainCommand_s &command provides torque info, which is used to calculate mechanical power + /// @param const DrivetrainDynamicReport_s &drivetrain provides RPMS, which are used to calculate radians / s + /// @param float max_torque is used to indirectly specifiy the max power DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); + + /// @brief begin limiting regen at noRegenLimitKPH (hardcoded in func) and completely limit regen at fullRegenLimitKPH (hardcoded in func) + /// @param const DrivetrainCommand_s &command + /// @param const DrivetrainDynamicReport_s &drivetrain_data provides RPMs DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); public: @@ -123,4 +138,4 @@ const int number_of_controllers = 5; using TCMuxType = TorqueControllerMux; #include "TorqueControllerMux.tpp" -#endif // __TorqueControllerMux_H__ \ No newline at end of file +#endif // __TorqueControllerMux_H__ From ad88f9edb18b1a9c1c8e02219fa62430fe8f3e5f Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sat, 21 Sep 2024 16:13:22 -0400 Subject: [PATCH 13/28] base contoller comments --- lib/systems/include/BaseController.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index a94556fb9..b4b3b3784 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -8,9 +8,16 @@ namespace BaseControllerParams .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}}; } +/** + * @brief Base class for all controller + * @note required method(s): evaluate + */ class Controller { public: + /// @brief ticks specific controller/system it belongs to + /// @param SharedCarState_s state + /// @return TorqueControllerOutput_s virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; }; From 8475adb810a1b36b5ef3b33c24e807de0a0bddbe Mon Sep 17 00:00:00 2001 From: Miles Kent Date: Sat, 21 Sep 2024 16:20:01 -0400 Subject: [PATCH 14/28] Added returns --- lib/systems/include/TorqueControllerMux.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index b8d0b7bfc..1a900699c 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -78,22 +78,26 @@ class TorqueControllerMux /// @brief Clamps negative rpms to 0f /// @param const DrivetrainCommand_s &command provides the rpm info as a DrivetrainCommand_s + /// @return DrivetrainCommand_s DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); /// @brief Ensure torque is at most at the specified limit. If exceeding, then limit it in the returned DrivetrainCommand_s /// @param const DrivetrainCommand_s &command is a DrivetrainCommand_s, which provides torque info /// @param float max_torque is the maximum average torque the wheels are allowed to experience before it is limited. + /// @return DrivetrainCommand_s DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); /// @brief 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 /// @param const DrivetrainCommand_s &command provides torque info, which is used to calculate mechanical power /// @param const DrivetrainDynamicReport_s &drivetrain provides RPMS, which are used to calculate radians / s /// @param float max_torque is used to indirectly specifiy the max power + /// @return DrivetrainCommand_s DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); /// @brief begin limiting regen at noRegenLimitKPH (hardcoded in func) and completely limit regen at fullRegenLimitKPH (hardcoded in func) /// @param const DrivetrainCommand_s &command /// @param const DrivetrainDynamicReport_s &drivetrain_data provides RPMs + /// @return DrivetrainCommand_s DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); public: From f8090634b421c9dbf013699762da0cfc5f2f61db Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sun, 22 Sep 2024 14:27:08 -0400 Subject: [PATCH 15/28] shared data types, casesystem, and casewrapper docs --- lib/shared_data/include/SharedDataTypes.h | 6 ++++++ lib/systems/include/CASESystem.h | 11 +++++++++-- .../include/Controllers/CASEControllerWrapper.h | 6 +++++- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 2e399a6df..9e0817afb 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -5,6 +5,7 @@ #include "SysClock.h" #include "SharedFirmwareTypes.h" +/// @brief defines modes of torque limit to be processed in torque limit map for exact values enum class TorqueLimit_e { TCMUX_FULL_TORQUE = 0, @@ -49,12 +50,15 @@ struct DrivetrainDynamicReport_s float measuredTorqueCurrents[NUM_MOTORS]; float measuredMagnetizingCurrents[NUM_MOTORS]; }; +/// @brief stores two arrays of size equal to the amount of motors: speed setpoints and necessary torque to reach those setpoints 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 }; +/// @brief Packages drivebrain command with ready boolean +///@note returned by all car controllers evaluate method struct TorqueControllerOutput_s { DrivetrainCommand_s command; @@ -80,6 +84,7 @@ struct VectornavData_s xyz_vec angular_rates; }; +/// @brief Defins errors for TC Mux to use to maintain system safety enum class TorqueControllerMuxError { NO_ERROR = 0, @@ -89,6 +94,7 @@ enum class TorqueControllerMuxError ERROR_CONTROLLER_NULL_POINTER =4 }; +/// @brief packages TC Mux indicators: errors, mode, torque limit, bypass struct TorqueControllerMuxStatus { TorqueControllerMuxError current_error; diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 5e5c962f2..a28f07198 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -148,10 +148,17 @@ class CASESystem DrivetrainCommand_s get_current_drive_command() { return current_command_; } + /// @brief uses pedal data to determine the requested torque by the car + /// @param PedalsSystemData_s &pedals_data + /// @param float max_torque + /// @param float max_regen_torque + /// @param float max_rpm + /// @return float representing the calculated request float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_torque, float max_regen_torque, float max_rpm); - /// @brief configuration function to determine what CASE is using / turn on and off different features within CASE - /// @param config the configuration struct we will be setting + /// @brief retrieves rpm setpoint based on final torque value + /// @param float final_torque + /// @return The maximum RPM from the configuration if torque is positive, otherwise 0. float get_rpm_setpoint(float final_torque) { if (final_torque > 0) diff --git a/lib/systems/include/Controllers/CASEControllerWrapper.h b/lib/systems/include/Controllers/CASEControllerWrapper.h index 8e0dbd7e8..7edc280e6 100644 --- a/lib/systems/include/Controllers/CASEControllerWrapper.h +++ b/lib/systems/include/Controllers/CASEControllerWrapper.h @@ -9,10 +9,14 @@ class TorqueControllerCASEWrapper : public virtual Controller { public: TorqueControllerCASEWrapper() = delete; - + /// @brief makes CASE system apart of Controller hierarchy for use in TC Mux + /// @param case_instance TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) { } + /// @brief packages CASE system command into Torque Controller Output + /// @param SharedCarState_s &state + /// @return TorqueControllerOutput_s TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { DrivetrainCommand_s curr_cmd = case_instance_->get_current_drive_command(); From 9da0a02365bad2574f78b844afd2fe83e4a74343 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sun, 22 Sep 2024 19:00:41 -0400 Subject: [PATCH 16/28] baselaunchcontroller --- lib/systems/include/BaseController.h | 2 ++ lib/systems/include/BaseLaunchController.h | 11 +++++++++++ 2 files changed, 13 insertions(+) diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index b4b3b3784..c9c52ba78 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -1,6 +1,8 @@ #ifndef BASECONTROLLER #define BASECONTROLLER #include "SharedDataTypes.h" + +/// @brief definition for a drivetrain command with no torque namespace BaseControllerParams { const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 0a3fbd4a4..cf189cb82 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -4,6 +4,8 @@ #include "BaseController.h" #include #include + +/// @brief Modes to guide tick behavior and progression enum class LaunchStates_e { NO_LAUNCH_MODE, @@ -12,6 +14,7 @@ enum class LaunchStates_e LAUNCHING }; +/// @brief contains constants for tick behavior and progression namespace BaseLaunchControllerParams { const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; @@ -37,6 +40,9 @@ class BaseLaunchController : public virtual Controller int16_t init_speed_target_ = 0; public: + /// @brief Constructor for template launch controller + /// @param initial_speed_target unused right now + /// @note requires one method: calc_launch_algo BaseLaunchController(int16_t initial_speed_target) : init_speed_target_(initial_speed_target) { @@ -44,6 +50,11 @@ class BaseLaunchController : public virtual Controller writeout_.ready = true; } + /// @brief ticks launch controller to progress through launch states when conditions are met + /// @param SysTick_s &tick + /// @param PedalsSystemData_s &pedalsData + /// @param float[] wheel_rpms + /// @param VectornavData_s &vn_data void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], From 844679269ca3b4beb75ced0e308e595a4078cdbb Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sun, 22 Sep 2024 22:25:48 -0400 Subject: [PATCH 17/28] finished checklist of documentation --- lib/shared_data/include/SharedDataTypes.h | 2 +- lib/systems/include/BaseLaunchController.h | 6 ++++++ .../include/Controllers/LoadCellVectoringController.h | 8 ++++++++ lib/systems/include/Controllers/LookupLaunchController.h | 5 +++-- lib/systems/include/Controllers/SimpleController.h | 6 +++++- lib/systems/include/Controllers/SimpleLaunchController.h | 7 ++++--- lib/systems/include/Controllers/SlipLaunchController.h | 5 +++-- 7 files changed, 30 insertions(+), 9 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 9e0817afb..59ca2cf44 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -57,7 +57,7 @@ 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 }; -/// @brief Packages drivebrain command with ready boolean +/// @brief Packages drivetrain command with ready boolean to give feedback on controller successfully evaluating ///@note returned by all car controllers evaluate method struct TorqueControllerOutput_s { diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index cf189cb82..9a7bdd730 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -60,7 +60,13 @@ class BaseLaunchController : public virtual Controller const float wheel_rpms[], const VectornavData_s &vn_data); LaunchStates_e get_launch_state() { return launch_state_; } + /// @brief calculates how speed target is set among launch controllers + /// @param VectornavData_s &vn_data + /// @note has important variation in launch controller tick as the launch controllers share a tick method virtual void calc_launch_algo(const VectornavData_s &vn_data) = 0; + /// @brief ticks launch controller + /// @param SharedCarState_s &state + /// @return TorqueControllerOutput_s TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; #endif // __BASELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/Controllers/LoadCellVectoringController.h b/lib/systems/include/Controllers/LoadCellVectoringController.h index dc7aaa373..d0344f4cc 100644 --- a/lib/systems/include/Controllers/LoadCellVectoringController.h +++ b/lib/systems/include/Controllers/LoadCellVectoringController.h @@ -57,12 +57,20 @@ class TorqueControllerLoadCellVectoring : public virtual Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = false; } + /// @brief default contructor with balanced dfault values: rearTorqueScale = 1.0, regenTorqueScale = 1.0 TorqueControllerLoadCellVectoring() : TorqueControllerLoadCellVectoring(1.0, 1.0) {} + /// @brief Calculates speed set point based on normal force applied to wheels individually + /// @param SysTick_s &tick + /// @param PedalsSystemData_s &pedalsData + /// @param LoadCellInterfaceOutput_s &loadCellData void tick( const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const LoadCellInterfaceOutput_s &loadCellData); + /// @brief ticks controller + /// @param SharedCarState_s &state + /// @return TorqueControllerOutput_s TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/LookupLaunchController.h b/lib/systems/include/Controllers/LookupLaunchController.h index 9072005fd..945eaa4f3 100644 --- a/lib/systems/include/Controllers/LookupLaunchController.h +++ b/lib/systems/include/Controllers/LookupLaunchController.h @@ -22,9 +22,10 @@ class TorqueControllerLookupLaunch : public virtual BaseLaunchController */ TorqueControllerLookupLaunch(int16_t initial_speed_target) : BaseLaunchController(initial_speed_target) {} - + /// @brief default constructor for slip launch controller: DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - + /// @brief increases speed target based on distance from start to ensure the speed target is progressing as the car begins to move + /// @param VectornavData_s &vn_data void calc_launch_algo(const VectornavData_s &vn_data) override; }; diff --git a/lib/systems/include/Controllers/SimpleController.h b/lib/systems/include/Controllers/SimpleController.h index 33f691fee..56c4f5f89 100644 --- a/lib/systems/include/Controllers/SimpleController.h +++ b/lib/systems/include/Controllers/SimpleController.h @@ -28,8 +28,12 @@ class TorqueControllerSimple : public Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = true; } - + /// @brief calculates torqe output based off max torque and simple torque scaling metric defined in constructor + /// @param PedalsSystemData_s &pedalsData void tick(const PedalsSystemData_s &pedalsData); + /// @brief ticks controller + /// @param SharedCarState_s &state + /// @return TorqueControllerOutput_s TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/SimpleLaunchController.h b/lib/systems/include/Controllers/SimpleLaunchController.h index ef4f84a64..7914e66ed 100644 --- a/lib/systems/include/Controllers/SimpleLaunchController.h +++ b/lib/systems/include/Controllers/SimpleLaunchController.h @@ -19,7 +19,7 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController public: /*! SIMPLE LAUNCH CONTROLLER - This launch controller is based off of a specified launch rate and an initial speed target + @brief 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 @@ -28,9 +28,10 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController : BaseLaunchController(initial_speed_target), launch_rate_target_(launch_rate) {} - + /// @brief base constrcutor with default values: Default_Launch_Rate = 11.76, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - + /// @brief Increases speed target during launch linearly + /// @param VectornavData_s &vn_data void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SIMPLELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/Controllers/SlipLaunchController.h b/lib/systems/include/Controllers/SlipLaunchController.h index f3b61eeb6..de9517c87 100644 --- a/lib/systems/include/Controllers/SlipLaunchController.h +++ b/lib/systems/include/Controllers/SlipLaunchController.h @@ -24,9 +24,10 @@ class TorqueControllerSlipLaunch : public virtual BaseLaunchController TorqueControllerSlipLaunch(float slip_ratio, int16_t initial_speed_target) : BaseLaunchController(initial_speed_target), slip_ratio_(slip_ratio) {} - + /// @brief default constructor for slip launch controller: DEFAULT_SLIP_RATIO = 0.2, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - + /// @brief Increases speed target during launch linearly according to slip ratio to keep the cars wheels spinning faster than the velocity for increased traction + /// @param VectornavData_s &vn_data void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file From 563400b74ba80a5ee686eefcb448c59b0700bcd4 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Sun, 22 Sep 2024 22:32:05 -0400 Subject: [PATCH 18/28] small change in baselaunchcontroller --- 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 9a7bdd730..720e0cc87 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -14,7 +14,7 @@ enum class LaunchStates_e LAUNCHING }; -/// @brief contains constants for tick behavior and progression +/// @brief contains constants for tick behavior/progression and defaults namespace BaseLaunchControllerParams { const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; From 98a9efc734d5d8a41777411f2739ccb7a752787d Mon Sep 17 00:00:00 2001 From: Miles Kent Date: Mon, 23 Sep 2024 20:11:52 -0400 Subject: [PATCH 19/28] Doxyfile Conf Update Automatically generate UML Diagrams based on the C++ files and headers. (Ben said this was okay) --- Doxyfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Doxyfile b/Doxyfile index 94c359474..3cf55d9e7 100644 --- a/Doxyfile +++ b/Doxyfile @@ -2481,7 +2481,7 @@ HIDE_UNDOC_RELATIONS = YES # set to NO # The default value is: NO. -HAVE_DOT = NO +HAVE_DOT = YES # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed # to run in parallel. When set to 0 doxygen will base this on the number of @@ -2565,7 +2565,7 @@ GROUP_GRAPHS = YES # The default value is: NO. # This tag requires that the tag HAVE_DOT is set to YES. -UML_LOOK = NO +UML_LOOK = YES # If the UML_LOOK tag is enabled, the fields and methods are shown inside the # class node. If there are many fields or methods and many nodes the graph may From cd2b0142271b78cfb2fc651ddd817ce8bb738fbb Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Tue, 24 Sep 2024 23:55:42 -0400 Subject: [PATCH 20/28] fixed requested changes --- lib/shared_data/include/SharedDataTypes.h | 12 ++++++---- lib/systems/include/BaseController.h | 8 +++---- lib/systems/include/BaseLaunchController.h | 22 +++++++++---------- lib/systems/include/CASESystem.h | 12 +++++----- .../Controllers/CASEControllerWrapper.h | 6 ++--- .../Controllers/LoadCellVectoringController.h | 6 ++--- .../include/Controllers/SimpleController.h | 8 +++---- .../Controllers/SimpleLaunchController.h | 6 ++--- lib/systems/include/TorqueControllerMux.h | 5 ++++- 9 files changed, 43 insertions(+), 42 deletions(-) diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 59ca2cf44..d386a112e 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -5,7 +5,7 @@ #include "SysClock.h" #include "SharedFirmwareTypes.h" -/// @brief defines modes of torque limit to be processed in torque limit map for exact values +/// @brief Defines modes of torque limit to be processed in torque limit map for exact values. enum class TorqueLimit_e { TCMUX_FULL_TORQUE = 0, @@ -50,7 +50,11 @@ struct DrivetrainDynamicReport_s float measuredTorqueCurrents[NUM_MOTORS]; float measuredMagnetizingCurrents[NUM_MOTORS]; }; -/// @brief stores two arrays of size equal to the amount of motors: speed setpoints and necessary torque to reach those setpoints +/// @brief Stores setpoints for a command to the Drivetrain, containing speed and torque setpoints for each motor. +/// The Speeds unit is rpm and are the targeted speeds for each wheel of the car. +/// The torques unit is nm and is the torque requested from the motors to reach such speeds. +/// One can use the arrays with FR(Front Left), FL(Front Left), RL(Rear Left), RR(Rear Right) to access or modify the respective set points. eg. speeds_rpm[FR] = 0.0; +/// Their indexes are defined in utility.h as follows: FL = 0, FR = 1, RL = 2, RR = 3. struct DrivetrainCommand_s { float speeds_rpm[NUM_MOTORS]; @@ -58,7 +62,7 @@ struct DrivetrainCommand_s }; /// @brief Packages drivetrain command with ready boolean to give feedback on controller successfully evaluating -///@note returned by all car controllers evaluate method +/// @note returned by all car controllers evaluate method struct TorqueControllerOutput_s { DrivetrainCommand_s command; @@ -84,7 +88,7 @@ struct VectornavData_s xyz_vec angular_rates; }; -/// @brief Defins errors for TC Mux to use to maintain system safety +/// @brief Defines errors for TC Mux to use to maintain system safety enum class TorqueControllerMuxError { NO_ERROR = 0, diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index c9c52ba78..922c6a9b9 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -2,7 +2,7 @@ #define BASECONTROLLER #include "SharedDataTypes.h" -/// @brief definition for a drivetrain command with no torque +/// @brief defines namespace for definition of a drivetrain command with no torque for clearer code in the Muxer namespace BaseControllerParams { const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { @@ -12,13 +12,13 @@ namespace BaseControllerParams } /** * @brief Base class for all controller - * @note required method(s): evaluate + * required method(s): evaluate */ class Controller { public: - /// @brief ticks specific controller/system it belongs to - /// @param SharedCarState_s state + /// @brief This ticks specific controller/system it belongs to. This is called in the Muxer whenever the drivetrain command is obtained. + /// @param state with all sensor information to properly define torque set points /// @return TorqueControllerOutput_s virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; }; diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 720e0cc87..03aa00739 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -5,7 +5,10 @@ #include #include -/// @brief Modes to guide tick behavior and progression +/// @brief Modes to define launch behavior, where each one waits for acceleration request threshold to move to next mode +/// LAUNCH_NOT_READY keeps speed at 0 and makes sure pedals are not pressed +/// LAUNCH_READY keeps speed at 0 and makes sure break is not pressed +/// LAUNCHING uses respective algorithm to set speed set point and requests torque from motors to reach it enum class LaunchStates_e { NO_LAUNCH_MODE, @@ -41,7 +44,7 @@ class BaseLaunchController : public virtual Controller public: /// @brief Constructor for template launch controller - /// @param initial_speed_target unused right now + /// @param initial_speed_target used only in simple launch controller algorithm /// @note requires one method: calc_launch_algo BaseLaunchController(int16_t initial_speed_target) : init_speed_target_(initial_speed_target) @@ -51,22 +54,17 @@ class BaseLaunchController : public virtual Controller } /// @brief ticks launch controller to progress through launch states when conditions are met - /// @param SysTick_s &tick - /// @param PedalsSystemData_s &pedalsData - /// @param float[] wheel_rpms - /// @param VectornavData_s &vn_data void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], const VectornavData_s &vn_data); LaunchStates_e get_launch_state() { return launch_state_; } - /// @brief calculates how speed target is set among launch controllers - /// @param VectornavData_s &vn_data - /// @note has important variation in launch controller tick as the launch controllers share a tick method + /// @brief calculates how speed target, the speed the car is trying to achieve during launch, is set and/or increased during launch + /// @param vn_data vector data like speed and coordinates + /// @note defines important variation in launch controller tick/evaluation as the launch controllers share a tick method defined in this parent class implementation + /// @note all launch algorithms are implemented in LaunchControllerAlgos.cpp virtual void calc_launch_algo(const VectornavData_s &vn_data) = 0; - /// @brief ticks launch controller - /// @param SharedCarState_s &state - /// @return TorqueControllerOutput_s + /// @note refer to parent class 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 a28f07198..64a6892a6 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -148,17 +148,17 @@ class CASESystem DrivetrainCommand_s get_current_drive_command() { return current_command_; } - /// @brief uses pedal data to determine the requested torque by the car - /// @param PedalsSystemData_s &pedals_data - /// @param float max_torque - /// @param float max_regen_torque - /// @param float max_rpm + /// @brief uses pedal data to determine the torque to be requested from the motors + /// @param pedals_data has accel and regen percent + /// @param max_torque not used right now + /// @param max_regen_torque used for calculation of torque request + /// @param max_rpm not used right now /// @return float representing the calculated request float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_torque, float max_regen_torque, float max_rpm); /// @brief retrieves rpm setpoint based on final torque value /// @param float final_torque - /// @return The maximum RPM from the configuration if torque is positive, otherwise 0. + /// @return The maximum RPM from the case configuration if torque is positive, otherwise 0. float get_rpm_setpoint(float final_torque) { if (final_torque > 0) diff --git a/lib/systems/include/Controllers/CASEControllerWrapper.h b/lib/systems/include/Controllers/CASEControllerWrapper.h index 7edc280e6..cb2d9cb1f 100644 --- a/lib/systems/include/Controllers/CASEControllerWrapper.h +++ b/lib/systems/include/Controllers/CASEControllerWrapper.h @@ -5,17 +5,17 @@ #include "CASESystem.h" template +/// @brief makes CASE system a part of Controller hierarchy for use in TC Mux class TorqueControllerCASEWrapper : public virtual Controller { public: TorqueControllerCASEWrapper() = delete; - /// @brief makes CASE system apart of Controller hierarchy for use in TC Mux - /// @param case_instance + /// @param case_instance provide current instance to ensure there are not duplicates of this system as it needs to stay updated for MCU to get data. TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) { } /// @brief packages CASE system command into Torque Controller Output - /// @param SharedCarState_s &state + /// @param state this state is updated by the CASE system in main repeatedly /// @return TorqueControllerOutput_s TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { diff --git a/lib/systems/include/Controllers/LoadCellVectoringController.h b/lib/systems/include/Controllers/LoadCellVectoringController.h index d0344f4cc..0b27d56bf 100644 --- a/lib/systems/include/Controllers/LoadCellVectoringController.h +++ b/lib/systems/include/Controllers/LoadCellVectoringController.h @@ -57,7 +57,7 @@ class TorqueControllerLoadCellVectoring : public virtual Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = false; } - /// @brief default contructor with balanced dfault values: rearTorqueScale = 1.0, regenTorqueScale = 1.0 + /// @brief default contructor with balanced default values: rearTorqueScale = 1.0, regenTorqueScale = 1.0 TorqueControllerLoadCellVectoring() : TorqueControllerLoadCellVectoring(1.0, 1.0) {} /// @brief Calculates speed set point based on normal force applied to wheels individually @@ -68,9 +68,7 @@ class TorqueControllerLoadCellVectoring : public virtual Controller const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const LoadCellInterfaceOutput_s &loadCellData); - /// @brief ticks controller - /// @param SharedCarState_s &state - /// @return TorqueControllerOutput_s + /// @note refer to parent class TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/SimpleController.h b/lib/systems/include/Controllers/SimpleController.h index 56c4f5f89..29bcaaef0 100644 --- a/lib/systems/include/Controllers/SimpleController.h +++ b/lib/systems/include/Controllers/SimpleController.h @@ -28,12 +28,10 @@ class TorqueControllerSimple : public Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = true; } - /// @brief calculates torqe output based off max torque and simple torque scaling metric defined in constructor - /// @param PedalsSystemData_s &pedalsData + /// @brief calculates torqe output based off max torque and simple torque scaling metric defined in constructor and adjusts writeout_ accordingly + /// @param pedalsData controller calculates acceleration request from the pedals based off of this data void tick(const PedalsSystemData_s &pedalsData); - /// @brief ticks controller - /// @param SharedCarState_s &state - /// @return TorqueControllerOutput_s + /// @note refer to parent class TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/SimpleLaunchController.h b/lib/systems/include/Controllers/SimpleLaunchController.h index 7914e66ed..e17d9b4a7 100644 --- a/lib/systems/include/Controllers/SimpleLaunchController.h +++ b/lib/systems/include/Controllers/SimpleLaunchController.h @@ -19,7 +19,7 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController public: /*! SIMPLE LAUNCH CONTROLLER - @brief launch controller is based off of a specified launch rate and an initial speed target + @brief 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 @@ -30,8 +30,8 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController /// @brief base constrcutor with default values: Default_Launch_Rate = 11.76, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} - /// @brief Increases speed target during launch linearly - /// @param VectornavData_s &vn_data + /// @brief Increases speed target during launch linearly based off launch rate provided in the constructor + /// @param vn_data this data is not necessary for this controller but is supplied according to the interface void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SIMPLELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 1a900699c..42eb30806 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -46,6 +46,8 @@ // - [ ] write testing code for this in separate environment +/// @brief Contains a max speed for mode changes(5 m/s), a max torque delta for mode change(.5 nm) and a max power limit(63000 W). +/// These values are used in the event that no value is provided for them in the constructor. namespace TC_MUX_DEFAULT_PARAMS { constexpr const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s @@ -109,6 +111,7 @@ class TorqueControllerMux /// @param max_change_speed the max speed difference between the requested controller output and the actual speed of each wheel that if the controller has a diff larger than the mux will not switch to the requested controller /// @param max_torque_pos_change_delta same as speed but evaluated between the controller commanded torques defaults to TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE /// @param max_power_limit the max power limit defaults to TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT + /// @note TC Mux must be created with at least 1 controller. explicit TorqueControllerMux(std::array controller_pointers, std::array mux_bypass_limits, float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, @@ -131,7 +134,7 @@ class TorqueControllerMux /// @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 + /// @return the current DrivetrainCommand_s to be sent to the drivetrain DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e controller_command_torque_limit, const SharedCarState_s &input_state); From 9ec4712bb8feba0f845e036fd24ce43bfc0ead22 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Wed, 25 Sep 2024 12:57:56 -0400 Subject: [PATCH 21/28] round three? --- lib/interfaces/src/MCUInterface.cpp | 6 +- lib/shared_data/include/SharedDataTypes.h | 15 ++-- lib/systems/include/BaseController.h | 15 ++-- lib/systems/include/BaseLaunchController.h | 27 ++++-- lib/systems/include/CASESystem.h | 1 + lib/systems/include/CASESystem.tpp | 8 +- .../Controllers/CASEControllerWrapper.h | 4 +- .../Controllers/LoadCellVectoringController.h | 2 +- .../Controllers/LookupLaunchController.h | 1 - .../include/Controllers/SimpleController.h | 4 +- .../Controllers/SimpleLaunchController.h | 4 +- .../Controllers/SlipLaunchController.h | 2 +- lib/systems/include/TorqueControllerMux.h | 26 +++--- lib/systems/include/TorqueControllerMux.tpp | 62 +++++++------- .../src/Controllers/BaseLaunchController.cpp | 24 +++--- .../src/Controllers/SimpleController.cpp | 16 ++-- src/main.cpp | 8 +- test/test_systems/tc_mux_v2_testing.h | 84 +++++++++---------- 18 files changed, 161 insertions(+), 148 deletions(-) diff --git a/lib/interfaces/src/MCUInterface.cpp b/lib/interfaces/src/MCUInterface.cpp index dabb3bc37..18464dc6f 100644 --- a/lib/interfaces/src/MCUInterface.cpp +++ b/lib/interfaces/src/MCUInterface.cpp @@ -212,9 +212,9 @@ void MCUInterface::tick(int fsm_state, update_mcu_status_CAN_drivetrain(inv_has_error); update_mcu_status_CAN_safety(software_is_ok); - auto drive_mode = static_cast(tc_mux_status.current_controller_mode); - auto torque_mode = static_cast(tc_mux_status.current_torque_limit_enum); - auto max_torque = tc_mux_status.current_torque_limit_value; + auto drive_mode = static_cast(tc_mux_status.active_controller_mode); + auto torque_mode = static_cast(tc_mux_status.active_torque_limit_enum); + auto max_torque = tc_mux_status.active_torque_limit_value; update_mcu_status_CAN_TCMux(drive_mode, torque_mode, max_torque); update_mcu_status_CAN_buzzer(buzzer_is_on); diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index d386a112e..e8978c772 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -50,15 +50,15 @@ struct DrivetrainDynamicReport_s float measuredTorqueCurrents[NUM_MOTORS]; float measuredMagnetizingCurrents[NUM_MOTORS]; }; -/// @brief Stores setpoints for a command to the Drivetrain, containing speed and torque setpoints for each motor. +/// @brief Stores setpoints for a command to the Drivetrain, containing speed and torque setpoints for each motor. These setpoints are defined in the torque controllers cycled by the TC Muxer. /// The Speeds unit is rpm and are the targeted speeds for each wheel of the car. -/// The torques unit is nm and is the torque requested from the motors to reach such speeds. +/// The torques unit is nm and is the max torque requested from the inverter to reach such speeds. /// One can use the arrays with FR(Front Left), FL(Front Left), RL(Rear Left), RR(Rear Right) to access or modify the respective set points. eg. speeds_rpm[FR] = 0.0; /// Their indexes are defined in utility.h as follows: FL = 0, FR = 1, RL = 2, RR = 3. struct DrivetrainCommand_s { float speeds_rpm[NUM_MOTORS]; - float torqueSetpoints[NUM_MOTORS]; // FIXME: misnomer. This represents the magnitude of the torque the inverter can command to reach the commanded speed setpoint + float inverter_torque_limit[NUM_MOTORS]; }; /// @brief Packages drivetrain command with ready boolean to give feedback on controller successfully evaluating @@ -68,6 +68,7 @@ struct TorqueControllerOutput_s DrivetrainCommand_s command; bool ready; }; + struct VectornavData_s { float velocity_x; @@ -101,10 +102,10 @@ enum class TorqueControllerMuxError /// @brief packages TC Mux indicators: errors, mode, torque limit, bypass struct TorqueControllerMuxStatus { - TorqueControllerMuxError current_error; - ControllerMode_e current_controller_mode; - TorqueLimit_e current_torque_limit_enum; - float current_torque_limit_value; + TorqueControllerMuxError active_error; + ControllerMode_e active_controller_mode; + TorqueLimit_e active_torque_limit_enum; + float active_torque_limit_value; bool output_is_bypassing_limits; }; diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h index 922c6a9b9..b532407bc 100644 --- a/lib/systems/include/BaseController.h +++ b/lib/systems/include/BaseController.h @@ -3,23 +3,24 @@ #include "SharedDataTypes.h" /// @brief defines namespace for definition of a drivetrain command with no torque for clearer code in the Muxer +/// This can be used to define other specific car states in the future namespace BaseControllerParams { const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, - .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}}; + .inverter_torque_limit = {0.0, 0.0, 0.0, 0.0}}; } -/** - * @brief Base class for all controller - * required method(s): evaluate - */ + /// @brief Base class for all controllers, which define drivetrain command containing different variations of +/// speed set points and necessary torque set points to be requested from the motors in order to achieve said speeds. + /// required method(s): evaluate class Controller { public: - /// @brief This ticks specific controller/system it belongs to. This is called in the Muxer whenever the drivetrain command is obtained. + /// @brief This mehod must be implemented by every controller in the Tc Muxer. This is called in the Muxer whenever the drivetrain command is obtained. + /// @ref TorqueControllerMux.cpp to see that in every tick of the system, the active controller must be ticked through this method /// @param state with all sensor information to properly define torque set points - /// @return TorqueControllerOutput_s + /// @return TorqueControllerOutput_s This is a Drivetrain command passed along with a state boolean to ensure car controllers are working properly virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; }; diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h index 03aa00739..f971a39ea 100644 --- a/lib/systems/include/BaseLaunchController.h +++ b/lib/systems/include/BaseLaunchController.h @@ -6,9 +6,13 @@ #include /// @brief Modes to define launch behavior, where each one waits for acceleration request threshold to move to next mode -/// LAUNCH_NOT_READY keeps speed at 0 and makes sure pedals are not pressed -/// LAUNCH_READY keeps speed at 0 and makes sure break is not pressed +/// LAUNCH_NOT_READY keeps speed at 0 and makes sure pedals are not pressed, the launch controller begins in this state +/// From this state the launch can only progress forwards to LAUNCH_READY +/// LAUNCH_READY keeps speed at 0, below the speed threshold(5 m/s) and makes sure brake is not pressed harder than the threshold of .2(20% pushed) +/// From this state the launch can progress forwards to LAUNCHING according to the two conditions defined above or backwards to LAUNCH_NOT_READY if those conditions are not met /// LAUNCHING uses respective algorithm to set speed set point and requests torque from motors to reach it +/// From this state the launch can fully begin and set speed set points above 0.0 m/s and the maximum available torque can be requested from the inverters +/// This launch state can be terminated if the brake is pressed above the threshold(.2(20% pushed)) or if the accelerator is not pressed enough (<= .5(50% pushed)) enum class LaunchStates_e { NO_LAUNCH_MODE, @@ -17,7 +21,7 @@ enum class LaunchStates_e LAUNCHING }; -/// @brief contains constants for tick behavior/progression and defaults +/// @brief contains constants for tick behavior/progression(_threshold variables used to determine when to move to the next step) and defaults(DEFAULT_LAUNCH_SPEED_TARGET) namespace BaseLaunchControllerParams { const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; @@ -43,7 +47,7 @@ class BaseLaunchController : public virtual Controller int16_t init_speed_target_ = 0; public: - /// @brief Constructor for template launch controller + /// @brief Constructor for parent launch controller /// @param initial_speed_target used only in simple launch controller algorithm /// @note requires one method: calc_launch_algo BaseLaunchController(int16_t initial_speed_target) @@ -53,18 +57,25 @@ class BaseLaunchController : public virtual Controller writeout_.ready = true; } - /// @brief ticks launch controller to progress through launch states when conditions are met + /// @brief ticks launch controller to progress through launch states when conditions are met. The conditions are explained above the launch states enum. + /// all launch controllers use this class' implementation of tick. + /// tick conatains the current system tick controlled by main.cpp + /// pedalsData conatins the brake and accelerator values + /// wheel_rpms[] contains how fast each wheel is spinning, order of wheels in this array is defined in SharedDataTypes.h and Utility.h + /// vn_data contains vector states of the car that will be provided to the calc_launch_algo method called in the LAUNCHING state of this set of modes void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const float wheel_rpms[], const VectornavData_s &vn_data); LaunchStates_e get_launch_state() { return launch_state_; } - /// @brief calculates how speed target, the speed the car is trying to achieve during launch, is set and/or increased during launch - /// @param vn_data vector data like speed and coordinates + /// @brief calculates how speed target (the speed the car is trying to achieve during launch) is set and/or increased during launch + /// This updates internal speed target variable launch_speed_target_ + /// @param vn_data vector data needed for calulations eg. speed and coordinates /// @note defines important variation in launch controller tick/evaluation as the launch controllers share a tick method defined in this parent class implementation /// @note all launch algorithms are implemented in LaunchControllerAlgos.cpp virtual void calc_launch_algo(const VectornavData_s &vn_data) = 0; - /// @note refer to parent class + /// @brief all launch controllers share the same evaluate method implemented in this class implementation. + /// @note refer to parent class for function documentation TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; #endif // __BASELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 64a6892a6..e0bc09c5a 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -149,6 +149,7 @@ class CASESystem DrivetrainCommand_s get_current_drive_command() { return current_command_; } /// @brief uses pedal data to determine the torque to be requested from the motors + /// pedals_data.accelPercent - pedals_data.regenPercent -> where accelpercent is to what percent the acellerator is pushed and the regen percent is the amount of regenerative braking currently applied /// @param pedals_data has accel and regen percent /// @param max_torque not used right now /// @param max_regen_torque used for calculation of torque request diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 1787cbb12..0071acf3e 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -268,10 +268,10 @@ DrivetrainCommand_s CASESystem::evaluate( DrivetrainCommand_s command; - command.torqueSetpoints[0] = res.FinalTorqueFL; - command.torqueSetpoints[1] = res.FinalTorqueFR; - command.torqueSetpoints[2] = res.FinalTorqueRL; - command.torqueSetpoints[3] = res.FinalTorqueRR; + command.inverter_torque_limit[0] = res.FinalTorqueFL; + command.inverter_torque_limit[1] = res.FinalTorqueFR; + command.inverter_torque_limit[2] = res.FinalTorqueRL; + command.inverter_torque_limit[3] = res.FinalTorqueRR; command.speeds_rpm[0] = get_rpm_setpoint(res.FinalTorqueFL); command.speeds_rpm[1] = get_rpm_setpoint(res.FinalTorqueFR); diff --git a/lib/systems/include/Controllers/CASEControllerWrapper.h b/lib/systems/include/Controllers/CASEControllerWrapper.h index cb2d9cb1f..5931780ac 100644 --- a/lib/systems/include/Controllers/CASEControllerWrapper.h +++ b/lib/systems/include/Controllers/CASEControllerWrapper.h @@ -10,13 +10,13 @@ class TorqueControllerCASEWrapper : public virtual Controller { public: TorqueControllerCASEWrapper() = delete; - /// @param case_instance provide current instance to ensure there are not duplicates of this system as it needs to stay updated for MCU to get data. + /// @param case_instance requires current state estimator instance to give access of the CASE instance to this specific controller so that it can update/control it. + /// @note This also ensures there are not duplicates of this system for safety. TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) { } /// @brief packages CASE system command into Torque Controller Output /// @param state this state is updated by the CASE system in main repeatedly - /// @return TorqueControllerOutput_s TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { DrivetrainCommand_s curr_cmd = case_instance_->get_current_drive_command(); diff --git a/lib/systems/include/Controllers/LoadCellVectoringController.h b/lib/systems/include/Controllers/LoadCellVectoringController.h index 0b27d56bf..9c7b59d47 100644 --- a/lib/systems/include/Controllers/LoadCellVectoringController.h +++ b/lib/systems/include/Controllers/LoadCellVectoringController.h @@ -68,7 +68,7 @@ class TorqueControllerLoadCellVectoring : public virtual Controller const SysTick_s &tick, const PedalsSystemData_s &pedalsData, const LoadCellInterfaceOutput_s &loadCellData); - /// @note refer to parent class + /// @note refer to parent class for function documentation TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/LookupLaunchController.h b/lib/systems/include/Controllers/LookupLaunchController.h index 945eaa4f3..3953289f6 100644 --- a/lib/systems/include/Controllers/LookupLaunchController.h +++ b/lib/systems/include/Controllers/LookupLaunchController.h @@ -25,7 +25,6 @@ class TorqueControllerLookupLaunch : public virtual BaseLaunchController /// @brief default constructor for slip launch controller: DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} /// @brief increases speed target based on distance from start to ensure the speed target is progressing as the car begins to move - /// @param VectornavData_s &vn_data void calc_launch_algo(const VectornavData_s &vn_data) override; }; diff --git a/lib/systems/include/Controllers/SimpleController.h b/lib/systems/include/Controllers/SimpleController.h index 29bcaaef0..16d67c1db 100644 --- a/lib/systems/include/Controllers/SimpleController.h +++ b/lib/systems/include/Controllers/SimpleController.h @@ -28,10 +28,10 @@ class TorqueControllerSimple : public Controller writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; writeout_.ready = true; } - /// @brief calculates torqe output based off max torque and simple torque scaling metric defined in constructor and adjusts writeout_ accordingly + /// @brief calculates torque output based off max torque and simple torque scaling metric defined in constructor and adjusts writeout_ accordingly /// @param pedalsData controller calculates acceleration request from the pedals based off of this data void tick(const PedalsSystemData_s &pedalsData); - /// @note refer to parent class + /// @note refer to parent class for function documentation TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; }; diff --git a/lib/systems/include/Controllers/SimpleLaunchController.h b/lib/systems/include/Controllers/SimpleLaunchController.h index e17d9b4a7..0d04df908 100644 --- a/lib/systems/include/Controllers/SimpleLaunchController.h +++ b/lib/systems/include/Controllers/SimpleLaunchController.h @@ -20,7 +20,7 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController /*! SIMPLE LAUNCH CONTROLLER @brief 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 + It will ramp up the speed target linearly over time to accelerate @param launch_rate specified launch rate in m/s^2 @param initial_speed_target the initial speed commanded to the wheels */ @@ -28,7 +28,7 @@ class TorqueControllerSimpleLaunch : public virtual BaseLaunchController : BaseLaunchController(initial_speed_target), launch_rate_target_(launch_rate) {} - /// @brief base constrcutor with default values: Default_Launch_Rate = 11.76, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) + /// @brief base constructor with default values: Default_Launch_Rate = 11.76, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} /// @brief Increases speed target during launch linearly based off launch rate provided in the constructor /// @param vn_data this data is not necessary for this controller but is supplied according to the interface diff --git a/lib/systems/include/Controllers/SlipLaunchController.h b/lib/systems/include/Controllers/SlipLaunchController.h index de9517c87..fd0e7c406 100644 --- a/lib/systems/include/Controllers/SlipLaunchController.h +++ b/lib/systems/include/Controllers/SlipLaunchController.h @@ -27,7 +27,7 @@ class TorqueControllerSlipLaunch : public virtual BaseLaunchController /// @brief default constructor for slip launch controller: DEFAULT_SLIP_RATIO = 0.2, DEFAULT_LAUNCH_SPEED_TARGET = 1500(rpm) TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} /// @brief Increases speed target during launch linearly according to slip ratio to keep the cars wheels spinning faster than the velocity for increased traction - /// @param VectornavData_s &vn_data + /// @param vn_data This controller needs velocity data for to keep updating increasing the speed according to the slip ratio void calc_launch_algo(const VectornavData_s &vn_data) override; }; #endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 42eb30806..30f646f4f 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -52,7 +52,7 @@ namespace TC_MUX_DEFAULT_PARAMS { constexpr const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s constexpr const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm - constexpr const float MAX_POWER_LIMIT = 63000.0; + constexpr const float MAX_POWER_LIMIT = 63000.0; // watts of mechanical power }; template @@ -73,33 +73,33 @@ class TorqueControllerMux {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0f}}; float max_change_speed_, max_torque_pos_change_delta_, max_power_limit_; DrivetrainCommand_s prev_command_ = {}; - TorqueControllerMuxStatus current_status_ = {}; - TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + TorqueControllerMuxStatus active_status_ = {}; + TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out); /// @brief Clamps negative rpms to 0f /// @param const DrivetrainCommand_s &command provides the rpm info as a DrivetrainCommand_s - /// @return DrivetrainCommand_s + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); /// @brief Ensure torque is at most at the specified limit. If exceeding, then limit it in the returned DrivetrainCommand_s /// @param const DrivetrainCommand_s &command is a DrivetrainCommand_s, which provides torque info - /// @param float max_torque is the maximum average torque the wheels are allowed to experience before it is limited. - /// @return DrivetrainCommand_s + /// @param float max_torque this is the maximum average torque the wheels are allowed to experience before it is limited. + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); - /// @brief Apply power limit 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 + /// @brief Apply power limit (watts) such that the mechanical power of all wheels never exceeds the preset mechanical power limit. Scales all wheels down to preserve functionality of torque controllers /// @param const DrivetrainCommand_s &command provides torque info, which is used to calculate mechanical power /// @param const DrivetrainDynamicReport_s &drivetrain provides RPMS, which are used to calculate radians / s /// @param float max_torque is used to indirectly specifiy the max power - /// @return DrivetrainCommand_s + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); /// @brief begin limiting regen at noRegenLimitKPH (hardcoded in func) and completely limit regen at fullRegenLimitKPH (hardcoded in func) /// @param const DrivetrainCommand_s &command /// @param const DrivetrainDynamicReport_s &drivetrain_data provides RPMs - /// @return DrivetrainCommand_s + /// @return DrivetrainCommand_s to update the drivetrain command in the getDrivetrainCommand method DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); public: @@ -128,13 +128,13 @@ class TorqueControllerMux } - const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } + const TorqueControllerMuxStatus &get_tc_mux_status() { return active_status_; } - /// @brief function that evaluates the mux, controllers and gets the current command + /// @brief function that evaluates the mux, controllers and gets the active command /// @param requested_controller_type the requested controller type from the dial state /// @param controller_command_torque_limit the torque limit state enum set by dashboard - /// @param input_state the current state of the car - /// @return the current DrivetrainCommand_s to be sent to the drivetrain + /// @param input_state the active state of the car + /// @return the active DrivetrainCommand_s to be sent to the drivetrain to command increases and decreases in torque DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, TorqueLimit_e controller_command_torque_limit, const SharedCarState_s &input_state); diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp index c392240bd..4d2e9699a 100644 --- a/lib/systems/include/TorqueControllerMux.tpp +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -13,24 +13,24 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C .ready = true}; int req_controller_mode_index = static_cast(requested_controller_type); - int current_controller_mode_index = static_cast(current_status_.current_controller_mode); + int active_controller_mode_index = static_cast(active_status_.active_controller_mode); if ((std::size_t)req_controller_mode_index > ( controller_pointers_.size() - 1 )) { - current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; + active_status_.active_error = TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS; return empty_command; } - if( (!(controller_pointers_[current_controller_mode_index])) || (!controller_pointers_[req_controller_mode_index])) + if( (!(controller_pointers_[active_controller_mode_index])) || (!controller_pointers_[req_controller_mode_index])) { - current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER; + active_status_.active_error = TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER; return empty_command; } - current_output = controller_pointers_[current_controller_mode_index]->evaluate(input_state); + current_output = controller_pointers_[active_controller_mode_index]->evaluate(input_state); - // std::cout << "output torques " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; - bool requesting_controller_change = requested_controller_type != current_status_.current_controller_mode; + // std::cout << "output torques " << current_output.command.inverter_torque_limit[0] << " " << current_output.command.inverter_torque_limit[1] << " " << current_output.command.inverter_torque_limit[2] << " " << current_output.command.inverter_torque_limit[3] << std::endl; + bool requesting_controller_change = requested_controller_type != active_status_.active_controller_mode; if (requesting_controller_change) { @@ -38,34 +38,34 @@ DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(C TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); if (error_state == TorqueControllerMuxError::NO_ERROR) { - current_status_.current_controller_mode = requested_controller_type; - current_controller_mode_index = req_controller_mode_index; + active_status_.active_controller_mode = requested_controller_type; + active_controller_mode_index = req_controller_mode_index; current_output = proposed_output; } - current_status_.current_error = error_state; + active_status_.active_error = error_state; } - if (!mux_bypass_limits_[current_controller_mode_index]) + if (!mux_bypass_limits_[active_controller_mode_index]) { - current_status_.current_torque_limit_enum = requested_torque_limit; + active_status_.active_torque_limit_enum = requested_torque_limit; current_output.command = apply_regen_limit_(current_output.command, input_state.drivetrain_data); current_output.command = apply_torque_limit_(current_output.command, torque_limit_map_[requested_torque_limit]); - current_status_.current_torque_limit_value = torque_limit_map_[requested_torque_limit]; + active_status_.active_torque_limit_value = torque_limit_map_[requested_torque_limit]; current_output.command = apply_power_limit_(current_output.command, input_state.drivetrain_data, max_power_limit_, torque_limit_map_[requested_torque_limit]); current_output.command = apply_positive_speed_limit_(current_output.command); - current_status_.output_is_bypassing_limits = false; + active_status_.output_is_bypassing_limits = false; } else{ - current_status_.current_torque_limit_enum = TorqueLimit_e::TCMUX_FULL_TORQUE; - current_status_.current_torque_limit_value= PhysicalParameters::AMK_MAX_TORQUE; - current_status_.output_is_bypassing_limits = true; + active_status_.active_torque_limit_enum = TorqueLimit_e::TCMUX_FULL_TORQUE; + active_status_.active_torque_limit_value= PhysicalParameters::AMK_MAX_TORQUE; + active_status_.output_is_bypassing_limits = true; } - // std::cout << "output torques before return " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; + // std::cout << "output torques before return " << current_output.command.inverter_torque_limit[0] << " " << current_output.command.inverter_torque_limit[1] << " " << current_output.command.inverter_torque_limit[2] << " " << current_output.command.inverter_torque_limit[3] << std::endl; return current_output.command; } template -TorqueControllerMuxError TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, +TorqueControllerMuxError TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s active_drivetrain_data, DrivetrainCommand_s previous_controller_command, DrivetrainCommand_s desired_controller_out) { @@ -74,9 +74,9 @@ TorqueControllerMuxError TorqueControllerMux::can_switch_contro bool torqueDeltaPreventsModeChange = false; for (int i = 0; i < NUM_MOTORS; i++) { - speedPreventsModeChange = (abs(current_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= max_change_speed_); + speedPreventsModeChange = (abs(active_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= max_change_speed_); // only if the torque delta is positive do we not want to switch to the new one - torqueDeltaPreventsModeChange = (desired_controller_out.torqueSetpoints[i] - previous_controller_command.torqueSetpoints[i]) > max_torque_pos_change_delta_; + torqueDeltaPreventsModeChange = (desired_controller_out.inverter_torque_limit[i] - previous_controller_command.inverter_torque_limit[i]) > max_torque_pos_change_delta_; if (speedPreventsModeChange) { return TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH; @@ -111,7 +111,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(co for (int i = 0; i < NUM_MOTORS; i++) { - avg_torque += abs(out.torqueSetpoints[i]); + avg_torque += abs(out.inverter_torque_limit[i]); } avg_torque /= NUM_MOTORS; @@ -123,7 +123,7 @@ DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(co // divide by scale to lower avg below max torque for (int i = 0; i < NUM_MOTORS; i++) { - out.torqueSetpoints[i] = out.torqueSetpoints[i] / scale; + out.inverter_torque_limit[i] = out.inverter_torque_limit[i] / scale; } } return out; @@ -146,9 +146,9 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con { // get the total magnitude of torque from all 4 wheels // sum up net torque - net_torque_mag += abs(out.torqueSetpoints[i]); + net_torque_mag += abs(out.inverter_torque_limit[i]); // calculate P = T*w for each wheel and sum together - net_power += abs(out.torqueSetpoints[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + net_power += abs(out.inverter_torque_limit[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); } // only evaluate power limit if current power exceeds it @@ -162,17 +162,17 @@ DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(con // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); // calculate the percent of total torque requested per wheel - float torque_percent = abs(out.torqueSetpoints[i] / net_torque_mag); + float torque_percent = abs(out.inverter_torque_limit[i] / net_torque_mag); // based on the torque percent and max power limit, get the max power each wheel can use float power_per_corner = (torque_percent * power_limit); // std::cout <<"corner power " << power_per_corner <::apply_regen_limit_(con { for (int i = 0; i < NUM_MOTORS; i++) { - out.torqueSetpoints[i] *= torqueScaleDown; + out.inverter_torque_limit[i] *= torqueScaleDown; } } return out; diff --git a/lib/systems/src/Controllers/BaseLaunchController.cpp b/lib/systems/src/Controllers/BaseLaunchController.cpp index c0bd646db..5a1ffab6c 100644 --- a/lib/systems/src/Controllers/BaseLaunchController.cpp +++ b/lib/systems/src/Controllers/BaseLaunchController.cpp @@ -31,10 +31,10 @@ void BaseLaunchController::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = brake_torque_req; - writeout_.command.torqueSetpoints[FR] = brake_torque_req; - writeout_.command.torqueSetpoints[RL] = brake_torque_req; - writeout_.command.torqueSetpoints[RR] = brake_torque_req; + writeout_.command.inverter_torque_limit[FL] = brake_torque_req; + writeout_.command.inverter_torque_limit[FR] = brake_torque_req; + writeout_.command.inverter_torque_limit[RL] = brake_torque_req; + writeout_.command.inverter_torque_limit[RR] = brake_torque_req; // init launch vars launch_speed_target_ = 0; @@ -53,10 +53,10 @@ void BaseLaunchController::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = brake_torque_req; - writeout_.command.torqueSetpoints[FR] = brake_torque_req; - writeout_.command.torqueSetpoints[RL] = brake_torque_req; - writeout_.command.torqueSetpoints[RR] = brake_torque_req; + writeout_.command.inverter_torque_limit[FL] = brake_torque_req; + writeout_.command.inverter_torque_limit[FR] = brake_torque_req; + writeout_.command.inverter_torque_limit[RL] = brake_torque_req; + writeout_.command.inverter_torque_limit[RR] = brake_torque_req; // init launch vars launch_speed_target_ = 0; @@ -94,10 +94,10 @@ void BaseLaunchController::tick( writeout_.command.speeds_rpm[RL] = launch_speed_target_; writeout_.command.speeds_rpm[RR] = launch_speed_target_; - writeout_.command.torqueSetpoints[FL] = PhysicalParameters::AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[FR] = PhysicalParameters::AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RL] = PhysicalParameters::AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RR] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[FL] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[FR] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[RL] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.inverter_torque_limit[RR] = PhysicalParameters::AMK_MAX_TORQUE; break; } diff --git a/lib/systems/src/Controllers/SimpleController.cpp b/lib/systems/src/Controllers/SimpleController.cpp index 4906e4ed8..0457cef95 100644 --- a/lib/systems/src/Controllers/SimpleController.cpp +++ b/lib/systems/src/Controllers/SimpleController.cpp @@ -18,10 +18,10 @@ void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) writeout_.command.speeds_rpm[RL] = PhysicalParameters::AMK_MAX_RPM; writeout_.command.speeds_rpm[RR] = PhysicalParameters::AMK_MAX_RPM; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; + writeout_.command.inverter_torque_limit[FL] = torqueRequest * frontTorqueScale_; + writeout_.command.inverter_torque_limit[FR] = torqueRequest * frontTorqueScale_; + writeout_.command.inverter_torque_limit[RL] = torqueRequest * rearTorqueScale_; + writeout_.command.inverter_torque_limit[RR] = torqueRequest * rearTorqueScale_; } else { @@ -33,10 +33,10 @@ void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RR] = torqueRequest * rearRegenTorqueScale_; } } diff --git a/src/main.cpp b/src/main.cpp index a8c2fe435..8e782a400 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -426,7 +426,7 @@ void loop() tick_all_systems(curr_tick); - // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode), curr_tick.millis, 100); + // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode), curr_tick.millis, 100); // inverter procedure before entering state machine // reset inverters if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) @@ -481,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().active_controller_mode)); Serial.print("Current TC error: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); Serial.println(); @@ -530,10 +530,10 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) int(fsm.get_state()), buzzer.buzzer_is_on(), drivetrain.drivetrain_error_occured(), - torque_controller_mux.get_tc_mux_status().current_torque_limit_enum, + torque_controller_mux.get_tc_mux_status().active_torque_limit_enum, ams_interface.get_filtered_min_cell_voltage(), a1.get().conversions[MCU15_GLV_SENSE_CHANNEL], - static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode), + static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode), dashboard.getDialMode()); main_ecu.tick( diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 7d7d1beae..529b6df92 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -25,10 +25,10 @@ void set_outputs(controller_type &controller, float mps, float torque) controller.output.command.speeds_rpm[1] = METERS_PER_SECOND_TO_RPM * mps; controller.output.command.speeds_rpm[2] = METERS_PER_SECOND_TO_RPM * mps; controller.output.command.speeds_rpm[3] = METERS_PER_SECOND_TO_RPM * mps; - controller.output.command.torqueSetpoints[0] = torque; - controller.output.command.torqueSetpoints[1] = torque; - controller.output.command.torqueSetpoints[2] = torque; - controller.output.command.torqueSetpoints[3] = torque; + controller.output.command.inverter_torque_limit[0] = torque; + controller.output.command.inverter_torque_limit[1] = torque; + controller.output.command.inverter_torque_limit[2] = torque; + controller.output.command.inverter_torque_limit[3] = torque; } template void set_output_rpm(controller_type &controller, float rpm, float torque) @@ -37,10 +37,10 @@ void set_output_rpm(controller_type &controller, float rpm, float torque) controller.output.command.speeds_rpm[1] = rpm; controller.output.command.speeds_rpm[2] = rpm; controller.output.command.speeds_rpm[3] = rpm; - controller.output.command.torqueSetpoints[0] = torque; - controller.output.command.torqueSetpoints[1] = torque; - controller.output.command.torqueSetpoints[2] = torque; - controller.output.command.torqueSetpoints[3] = torque; + controller.output.command.inverter_torque_limit[0] = torque; + controller.output.command.inverter_torque_limit[1] = torque; + controller.output.command.inverter_torque_limit[2] = torque; + controller.output.command.inverter_torque_limit[3] = torque; } TEST(TorqueControllerMuxTesting, test_construction) { @@ -60,7 +60,7 @@ TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) { ASSERT_EQ(res.speeds_rpm[i], 0.0); - ASSERT_EQ(res.torqueSetpoints[i], 0.0); + ASSERT_EQ(res.inverter_torque_limit[i], 0.0); } } @@ -84,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().active_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().active_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().active_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,12 +120,12 @@ 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().active_controller_mode, ControllerMode_e::MODE_0); - ASSERT_EQ(out1.torqueSetpoints[0], 1); - ASSERT_EQ(out1.torqueSetpoints[1], 1); - ASSERT_EQ(out1.torqueSetpoints[2], 1); - ASSERT_EQ(out1.torqueSetpoints[3], 1); + ASSERT_EQ(out1.inverter_torque_limit[0], 1); + ASSERT_EQ(out1.inverter_torque_limit[1], 1); + ASSERT_EQ(out1.inverter_torque_limit[2], 1); + ASSERT_EQ(out1.inverter_torque_limit[3], 1); } TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) @@ -179,17 +179,17 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) SharedCarState_s mode_0_input_state({}, {}, {}, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); DrivetrainCommand_s out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); - ASSERT_NEAR(out.torqueSetpoints[0], (max_torque / 2), 0.01); - ASSERT_NEAR(out.torqueSetpoints[1], (max_torque / 2), 0.01); - ASSERT_NEAR(out.torqueSetpoints[2], (max_torque / 2), 0.01); - ASSERT_NEAR(out.torqueSetpoints[3], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[0], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[1], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[2], (max_torque / 2), 0.01); + ASSERT_NEAR(out.inverter_torque_limit[3], (max_torque / 2), 0.01); mode_0_input_state = {{}, {}, {}, {}, {.accelPercent = 0.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}}; out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); - ASSERT_EQ(out.torqueSetpoints[0], 0); - ASSERT_EQ(out.torqueSetpoints[1], 0); - ASSERT_EQ(out.torqueSetpoints[2], 0); - ASSERT_EQ(out.torqueSetpoints[3], 0); + ASSERT_EQ(out.inverter_torque_limit[0], 0); + ASSERT_EQ(out.inverter_torque_limit[1], 0); + ASSERT_EQ(out.inverter_torque_limit[2], 0); + ASSERT_EQ(out.inverter_torque_limit[3], 0); // out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_1_input_state); } @@ -211,7 +211,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) for (int i = 0; i < 4; i++) { - ASSERT_EQ(res.torqueSetpoints[i], 10.0f); + ASSERT_EQ(res.inverter_torque_limit[i], 10.0f); } for (int i = 0; i < 4; i++) { @@ -224,7 +224,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) for (int i = 0; i < 4; i++) { - ASSERT_LT(res.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator + ASSERT_LT(res.inverter_torque_limit[i], 7.6); // hardcoded value based on online calculator } } @@ -234,7 +234,7 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) TestControllerType inst1; set_output_rpm(inst1, 500, 10.0); - inst1.output.command.torqueSetpoints[0] = 5; + inst1.output.command.inverter_torque_limit[0] = 5; TorqueControllerMux<1> test({static_cast(&inst1)}, {false}); DrivetrainDynamicReport_s drivetrain_data = {}; @@ -247,25 +247,25 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) auto drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); - ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); - ASSERT_EQ(drive_command.torqueSetpoints[1], 10.0f); - ASSERT_EQ(drive_command.torqueSetpoints[2], 10.0f); - ASSERT_EQ(drive_command.torqueSetpoints[3], 10.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[0], 5.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[1], 10.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[2], 10.0f); + ASSERT_EQ(drive_command.inverter_torque_limit[3], 10.0f); set_output_rpm(inst1, 500, 20.0); - inst1.output.command.torqueSetpoints[0] = 5; + inst1.output.command.inverter_torque_limit[0] = 5; drive_command = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, mode_0_input_state); - ASSERT_LT(drive_command.torqueSetpoints[0], 3.5f); - ASSERT_LT(drive_command.torqueSetpoints[1], 12.5f); - ASSERT_LT(drive_command.torqueSetpoints[2], 12.5f); - ASSERT_LT(drive_command.torqueSetpoints[3], 12.5f); + ASSERT_LT(drive_command.inverter_torque_limit[0], 3.5f); + ASSERT_LT(drive_command.inverter_torque_limit[1], 12.5f); + ASSERT_LT(drive_command.inverter_torque_limit[2], 12.5f); + ASSERT_LT(drive_command.inverter_torque_limit[3], 12.5f); - printf("torque 1: %.2f\n", drive_command.torqueSetpoints[0]); - printf("torque 2: %.2f\n", drive_command.torqueSetpoints[1]); - printf("torque 3: %.2f\n", drive_command.torqueSetpoints[2]); - printf("torque 4: %.2f\n", drive_command.torqueSetpoints[3]); + printf("torque 1: %.2f\n", drive_command.inverter_torque_limit[0]); + printf("torque 2: %.2f\n", drive_command.inverter_torque_limit[1]); + printf("torque 3: %.2f\n", drive_command.inverter_torque_limit[2]); + printf("torque 4: %.2f\n", drive_command.inverter_torque_limit[3]); } TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) @@ -275,7 +275,7 @@ TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, state); for (int i = 0; i < 4; i++) { - ASSERT_EQ(res.torqueSetpoints[i], 0.0f); + ASSERT_EQ(res.inverter_torque_limit[i], 0.0f); ASSERT_EQ(res.speeds_rpm[i], 0.0f); } ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); From 7589dc2da2c990131bfcc51d9ce1e092f46497f6 Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Wed, 25 Sep 2024 13:32:58 -0400 Subject: [PATCH 22/28] finished refactoring so pio works --- lib/systems/include/DrivetrainSystem.tpp | 2 +- .../Controllers/LoadCellVectoringController.cpp | 16 ++++++++-------- test/test_systems/tc_mux_v2_testing.h | 13 +++++++------ 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index de01cc702..5c2b52356 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -148,7 +148,7 @@ void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_ int index = 0; for (auto inv_pointer : inverters_) { - inv_pointer->handle_command({data.torqueSetpoints[index], data.speeds_rpm[index]}); + inv_pointer->handle_command({data.inverter_torque_limit[index], data.speeds_rpm[index]}); index++; } // last_general_cmd_time_ = curr_system_millis_; diff --git a/lib/systems/src/Controllers/LoadCellVectoringController.cpp b/lib/systems/src/Controllers/LoadCellVectoringController.cpp index e85502e84..5643df325 100644 --- a/lib/systems/src/Controllers/LoadCellVectoringController.cpp +++ b/lib/systems/src/Controllers/LoadCellVectoringController.cpp @@ -64,10 +64,10 @@ void TorqueControllerLoadCellVectoring::tick( writeout_.command.speeds_rpm[RL] = PhysicalParameters::AMK_MAX_RPM; writeout_.command.speeds_rpm[RR] = PhysicalParameters::AMK_MAX_RPM; - writeout_.command.torqueSetpoints[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; - writeout_.command.torqueSetpoints[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; - writeout_.command.torqueSetpoints[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; - writeout_.command.torqueSetpoints[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; + writeout_.command.inverter_torque_limit[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; + writeout_.command.inverter_torque_limit[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; + writeout_.command.inverter_torque_limit[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; + writeout_.command.inverter_torque_limit[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; } else { @@ -80,10 +80,10 @@ void TorqueControllerLoadCellVectoring::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.inverter_torque_limit[RR] = torqueRequest * rearRegenTorqueScale_; } } else diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 529b6df92..5347a6745 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -4,6 +4,7 @@ #include "TorqueControllerMux.h" #include "TorqueControllers.h" #include "fake_controller_type.h" +#include "gtest/gtest.h" @@ -55,7 +56,7 @@ TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) SharedCarState_s state({}, {}, {}, {}, {}, {}); auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); for (int i =0; i< 4; i++) { @@ -85,7 +86,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().active_controller_mode, ControllerMode_e::MODE_0); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH); set_outputs(inst1, 0, 1); set_outputs(inst2, 0, 1); @@ -94,7 +95,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().active_controller_mode, ControllerMode_e::MODE_1); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::NO_ERROR); } TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) @@ -108,7 +109,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().active_controller_mode, ControllerMode_e::MODE_0); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); // tick it a bunch of times out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); @@ -118,7 +119,7 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); ASSERT_EQ(test.get_tc_mux_status().active_controller_mode, ControllerMode_e::MODE_0); @@ -278,6 +279,6 @@ TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) ASSERT_EQ(res.inverter_torque_limit[i], 0.0f); ASSERT_EQ(res.speeds_rpm[i], 0.0f); } - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); + ASSERT_EQ(test.get_tc_mux_status().active_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); } #endif // __TC_MUX_V2_TESTING_H__ \ No newline at end of file From 5cd26fb147b0793d1593be299e71b81014b2d61f Mon Sep 17 00:00:00 2001 From: Comerm28 Date: Wed, 25 Sep 2024 13:36:37 -0400 Subject: [PATCH 23/28] build on teensy works --- src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 8e782a400..0f7c2e608 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -483,7 +483,7 @@ void loop() Serial.print("Current TC index: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode)); Serial.print("Current TC error: "); - Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); + Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_error)); Serial.println(); Serial.println(); } @@ -566,7 +566,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE1_CHANNEL], a1.get().conversions[MCU15_BRAKE2_CHANNEL], pedals_system.getMechBrakeActiveThreshold(), - torque_controller_mux.get_tc_mux_status().current_error); + torque_controller_mux.get_tc_mux_status().active_error); } if (t.trigger50) // 50Hz From 9879f78a2ade6374fbcfb55826ce81d9eefb2627 Mon Sep 17 00:00:00 2001 From: Miles Kent Date: Sat, 28 Sep 2024 19:29:00 -0400 Subject: [PATCH 24/28] Dep: graphviz Make sure autoUML deps are satisfied --- .github/workflows/docs.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index fe7ed526b..d3a5d166a 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -17,6 +17,9 @@ jobs: uses: mattnotmitt/doxygen-action@v1.9.5 with: doxyfile-path: 'Doxyfile' + + - name: Install dependencies + run: sudo apt-get update && sudo apt-get install -y graphviz - name: Deploy to GitHub Pages uses: peaceiris/actions-gh-pages@v3 From f1089beb86c37825f44bbd5ba8661fa7aa6fadf1 Mon Sep 17 00:00:00 2001 From: Miles Kent <64712073+mileskent@users.noreply.github.com> Date: Sun, 29 Sep 2024 02:41:02 +0000 Subject: [PATCH 25/28] Revert "Doxyfile Conf Update" --- .github/workflows/docs.yml | 3 - Doxyfile | 4 +- lib/systems/include/TorqueControllerMux.h | 1 + test/test_systems/tc_mux_v2_testing.h | 324 ++++++---------------- 4 files changed, 84 insertions(+), 248 deletions(-) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index d3a5d166a..fe7ed526b 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -17,9 +17,6 @@ jobs: uses: mattnotmitt/doxygen-action@v1.9.5 with: doxyfile-path: 'Doxyfile' - - - name: Install dependencies - run: sudo apt-get update && sudo apt-get install -y graphviz - name: Deploy to GitHub Pages uses: peaceiris/actions-gh-pages@v3 diff --git a/Doxyfile b/Doxyfile index 3cf55d9e7..94c359474 100644 --- a/Doxyfile +++ b/Doxyfile @@ -2481,7 +2481,7 @@ HIDE_UNDOC_RELATIONS = YES # set to NO # The default value is: NO. -HAVE_DOT = YES +HAVE_DOT = NO # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed # to run in parallel. When set to 0 doxygen will base this on the number of @@ -2565,7 +2565,7 @@ GROUP_GRAPHS = YES # The default value is: NO. # This tag requires that the tag HAVE_DOT is set to YES. -UML_LOOK = YES +UML_LOOK = NO # If the UML_LOOK tag is enabled, the fields and methods are shown inside the # class node. If there are many fields or methods and many nodes the graph may diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 21d89efb2..1db0cfb80 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -75,6 +75,7 @@ class TorqueControllerMux 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); diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h index 086fc108b..7d7d1beae 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -4,7 +4,6 @@ #include "TorqueControllerMux.h" #include "TorqueControllers.h" #include "fake_controller_type.h" -#include @@ -43,73 +42,29 @@ void set_output_rpm(controller_type &controller, float rpm, float torque) controller.output.command.torqueSetpoints[2] = torque; controller.output.command.torqueSetpoints[3] = torque; } - -//construction testing TEST(TorqueControllerMuxTesting, test_construction) { TestControllerType inst1, inst2; TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); } -TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) -{ - // mode 0 - TorqueControllerSimple tc_simple(1.0f, 1.0f); - // mode 1 - TorqueControllerLoadCellVectoring tc_vec; - // mode 2 - DummyQueue_s 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}); -} -TEST(TorqueControllerMuxTesting, test_construction_bypass_limits) + +TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) { TestControllerType inst1, inst2; - set_output_rpm(inst1, 500, 30.0); - set_output_rpm(inst2, 500, 30.0); - TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {true, false}); - - DrivetrainDynamicReport_s drivetrain_data = {}; - for (int i = 0; i < 4; i++) + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + 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); + for (int i =0; i< 4; i++) { - drivetrain_data.measuredSpeeds[i] = 500.0f; + + ASSERT_EQ(res.speeds_rpm[i], 0.0); + ASSERT_EQ(res.torqueSetpoints[i], 0.0); } - //onlhy use mode 0 for the input state in all tests and then test if they would act the same, idk how to do that - 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); - - ASSERT_EQ(drive_command.torqueSetpoints[0], 30.0f); - ASSERT_EQ(drive_command.torqueSetpoints[1], 30.0f); - ASSERT_EQ(drive_command.torqueSetpoints[2], 30.0f); - ASSERT_EQ(drive_command.torqueSetpoints[3], 30.0f); - - ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true); - - inst1.output.command.torqueSetpoints[0] = 5; - 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], 30.0f); - ASSERT_EQ(drive_command.torqueSetpoints[2], 30.0f); - ASSERT_EQ(drive_command.torqueSetpoints[3], 30.0f); - - ASSERT_EQ(test.get_tc_mux_status().output_is_bypassing_limits, true); } - -//logic testing + + // 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) @@ -120,82 +75,28 @@ TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); SharedCarState_s 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); - + // 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, simple_switch_many_active_modes) -{ - TestControllerType inst1, inst2, inst3, inst4; - set_outputs(inst1, 0, 1); - set_outputs(inst2, 0, 1); - set_outputs(inst3, 0, 1); - set_outputs(inst4, 0, 1); - TorqueControllerMux<4> test({static_cast(&inst1), static_cast(&inst2), static_cast(&inst3), static_cast(&inst4)}, {false, false, false, false}); - SharedCarState_s state({}, {}, {}, {}, {}, {}); - set_four_outputs(state.drivetrain_data.measuredSpeeds, 2); - - state.pedals_data = {}; - state.vn_data = {}; - - auto out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, 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::NO_ERROR); - - 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); - - out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - - out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - - out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); - ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - - out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_0, 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::NO_ERROR); -} - -//TorqueControllerMuxError testing -TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) -{ - TestControllerType inst1, inst2; - TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - 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); - for (int i =0; i< 4; i++) - { - - ASSERT_EQ(res.speeds_rpm[i], 0.0); - ASSERT_EQ(res.torqueSetpoints[i], 0.0); - } -} + TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) { TestControllerType inst1, inst2; @@ -203,12 +104,12 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) set_outputs(inst2, 3, 10); TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); 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); 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); @@ -216,35 +117,45 @@ TEST(TorqueControllerMuxTesting, test_torque_diff_swap_limit) out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); 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_null_pointer_error_state) + +TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) { - TorqueControllerMux<1> 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); - } - //makes it not able to test, assume i didnt pull most recent code - // ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER); + // mode 0 + TorqueControllerSimple tc_simple(1.0f, 1.0f); + // mode 1 + TorqueControllerLoadCellVectoring tc_vec; + // mode 2 + DummyQueue_s 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}); } - -//make generic -//mode evalutaion testing + TEST(TorqueControllerMuxTesting, test_mode0_evaluation) { - + float max_torque = 21.42; // mode 0 TorqueControllerSimple tc_simple(1.0f, 1.0f); @@ -254,7 +165,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) DummyQueue_s q; CASESystem case_sys(&q, 100, 70, 550, {}); TorqueControllerCASEWrapper case_wrapper(&case_sys); - + // mode 3 TorqueControllerSimpleLaunch simple_launch; // mode 4 @@ -266,39 +177,38 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) static_cast(&slip_launch)}, {false, true, false, false, false}); SharedCarState_s mode_0_input_state({}, {}, {}, {}, {.accelPercent = 0.5f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}); - + DrivetrainCommand_s out = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, mode_0_input_state); ASSERT_NEAR(out.torqueSetpoints[0], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[1], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[2], (max_torque / 2), 0.01); ASSERT_NEAR(out.torqueSetpoints[3], (max_torque / 2), 0.01); - + 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); } - -//limit testing + 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; } 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); - + for (int i = 0; i < 4; i++) { ASSERT_EQ(res.torqueSetpoints[i], 10.0f); @@ -308,138 +218,66 @@ TEST(TorqueControllerMuxTesting, test_power_limit) drivetrain_data.measuredSpeeds[i] = 20000.0f; } set_output_rpm(inst1, 20000, 21.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++) { 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; } - + 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); - + 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]); } -//integration testing -TEST(TorqueControllerMuxTesting, test_tc_mux_status) -{ - TorqueControllerMuxError testErr = TorqueControllerMuxError::NO_ERROR; - ControllerMode_e testMode = ControllerMode_e::MODE_0; - TorqueLimit_e testTorqLim = TorqueLimit_e::TCMUX_FULL_TORQUE; - TestControllerType inst1; - set_outputs(inst1, 0, 1); - TorqueControllerMux<1> test({static_cast(&inst1)}, { false }); - SharedCarState_s state({}, {}, {}, {}, {}, {}); - set_four_outputs(state.drivetrain_data.measuredSpeeds, 2); - - state.pedals_data = {}; - state.vn_data = {}; - - auto out1 = test.getDrivetrainCommand(testMode, testTorqLim, state); - ASSERT_EQ(test.get_tc_mux_status().current_error, testErr); - ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, testMode); - ASSERT_EQ(test.get_tc_mux_status().current_torque_limit_enum, testTorqLim); -} -TEST(TorqueControllerMuxTesting, test_real_controllers_output) +TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) { - //mode 0 - TorqueControllerSimple tc_simple(1.0f, 1.0f); - // mode 1 - TorqueControllerLoadCellVectoring tc_vec; - // mode 2 - DummyQueue_s 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, false, false, false}); + TorqueControllerMux<1> test({nullptr}, {true}); SharedCarState_s state({}, {}, {}, {}, {}, {}); - state.pedals_data = {}; - state.vn_data = {}; - state.drivetrain_data = {}; - - auto out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_1); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(static_cast(torque_controller_mux.get_tc_mux_status().current_error), static_cast(TorqueControllerMuxError::NO_ERROR)); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_4); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_4); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_3); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_2, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_2); - - out1 = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, state); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_error, TorqueControllerMuxError::NO_ERROR); - ASSERT_EQ(torque_controller_mux.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); + 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 2be1f0d51168de093b4300f85dbc889efd55ec39 Mon Sep 17 00:00:00 2001 From: Miles Kent <64712073+mileskent@users.noreply.github.com> Date: Sun, 29 Sep 2024 16:05:43 +0000 Subject: [PATCH 26/28] doxy real auto UML (#109) --- .github/workflows/docs.yml | 4 ++++ Doxyfile | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index fe7ed526b..9ba135828 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -17,6 +17,10 @@ jobs: uses: mattnotmitt/doxygen-action@v1.9.5 with: doxyfile-path: 'Doxyfile' + + + - name: Install dependencies + run: sudo apt-get update && sudo apt-get install -y graphviz - name: Deploy to GitHub Pages uses: peaceiris/actions-gh-pages@v3 diff --git a/Doxyfile b/Doxyfile index 94c359474..3cf55d9e7 100644 --- a/Doxyfile +++ b/Doxyfile @@ -2481,7 +2481,7 @@ HIDE_UNDOC_RELATIONS = YES # set to NO # The default value is: NO. -HAVE_DOT = NO +HAVE_DOT = YES # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed # to run in parallel. When set to 0 doxygen will base this on the number of @@ -2565,7 +2565,7 @@ GROUP_GRAPHS = YES # The default value is: NO. # This tag requires that the tag HAVE_DOT is set to YES. -UML_LOOK = NO +UML_LOOK = YES # If the UML_LOOK tag is enabled, the fields and methods are shown inside the # class node. If there are many fields or methods and many nodes the graph may From c63146583013030b526361040ec7fb1ff4475aa1 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Fri, 18 Oct 2024 17:57:33 -0400 Subject: [PATCH 27/28] Feature/drivebrain integration (#105) * tuning pedals and updated to latest can definition * drivebrain controller creation * added receiving of drivebrain messages * fixing pedals tests * adding drivebrain interface empty mock header * feat(drivebrain_controller): added tests for drivebrain controller latency and jitter * adding in latching logic for the db controller * added in drivebrain interface into main * ensuring that the regen and torque limits dont get applied to the drivebrain controller * fix(drivebrain_controller): fixed drivebrain_controller failure and reset logic * adjusting logic; removing interaction with tc and giving drivebrain more responsbility * fixing oopsie in drivebrain control mode and adding it to main * ready to test on car * fixing header * seemingly working ethernet integration * switching ethernet driver * removing CAN based drivebrain interface, addressing naming for inverter interface updates, updating ethernet interface since only one message is received, and fixing tests for new interface * making the veh_vec constuctor actually a constructor * addressing comments * a little cleanup * added compiler macro flag for debug prints * added in type alias * adding in better handling of timing for drivebrain message latency * updated to latest HT-prot * updating pedal params and to latest HT_proto --------- Co-authored-by: sreekara --- include/InterfaceParams.h | 7 +- include/MCU_rev15_defs.h | 10 +- .../include/DrivebrainETHInterface.h | 24 ++++ lib/interfaces/include/HytechCANInterface.h | 3 +- lib/interfaces/include/InverterInterface.h | 6 +- lib/interfaces/include/InverterInterface.tpp | 7 +- lib/interfaces/include/LoadCellInterface.h | 13 +- lib/interfaces/include/ProtobufMsgInterface.h | 45 +++--- lib/interfaces/include/TelemetryInterface.h | 9 +- lib/interfaces/library.json | 3 +- lib/interfaces/src/DrivebrainETHInterface.cpp | 31 +++++ lib/interfaces/src/TelemetryInterface.cpp | 26 +++- lib/mock_interfaces/DrivebrainInterface.h | 5 + lib/shared_data/include/DrivebrainData.h | 16 +++ lib/shared_data/include/SharedDataTypes.h | 18 ++- lib/shared_data/include/Utility.h | 31 ++++- lib/state_machine/include/MCUStateMachine.h | 2 - lib/state_machine/include/MCUStateMachine.tpp | 45 ------ lib/systems/include/DrivebrainController.h | 47 +++++++ lib/systems/include/DrivetrainSystem.tpp | 4 +- lib/systems/include/TorqueControllerMux.h | 1 + lib/systems/src/DrivebrainController.cpp | 49 +++++++ platformio.ini | 12 +- src/main.cpp | 128 +++++++++--------- .../test_systems/drivebrain_controller_test.h | 63 +++++++++ .../launch_controller_integration_testing.h | 30 ++-- test/test_systems/main.cpp | 1 + test/test_systems/state_machine_test.h | 2 +- test/test_systems/tc_mux_v2_testing.h | 18 +-- 29 files changed, 458 insertions(+), 198 deletions(-) create mode 100644 lib/interfaces/include/DrivebrainETHInterface.h create mode 100644 lib/interfaces/src/DrivebrainETHInterface.cpp create mode 100644 lib/mock_interfaces/DrivebrainInterface.h create mode 100644 lib/shared_data/include/DrivebrainData.h create mode 100644 lib/systems/include/DrivebrainController.h create mode 100644 lib/systems/src/DrivebrainController.cpp create mode 100644 test/test_systems/drivebrain_controller_test.h diff --git a/include/InterfaceParams.h b/include/InterfaceParams.h index 0c55b6e92..77d8f9e7c 100644 --- a/include/InterfaceParams.h +++ b/include/InterfaceParams.h @@ -1,6 +1,9 @@ #ifndef INTERFACEPARAMS #define INTERFACEPARAMS -#include "NativeEthernet.h" + +#include + +using namespace qindesign::network; namespace EthParams { @@ -8,7 +11,7 @@ namespace EthParams {0x04, 0xe9, 0xe5, 0x10, 0x1f, 0x22}; const IPAddress default_MCU_ip(192, 168, 1, 30); - const IPAddress default_TCU_ip(192, 168, 1, 68); + const IPAddress default_TCU_ip(192, 168, 1, 69); const uint16_t default_protobuf_send_port = 2001; const uint16_t default_protobuf_recv_port = 2000; diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index a11ee99dc..b651397eb 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -52,14 +52,14 @@ 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 = 3491; +const int ACCEL1_PEDAL_MAX = 3490; const int ACCEL2_PEDAL_MAX = 189; -const int ACCEL1_PEDAL_MIN = 2244; -const int ACCEL2_PEDAL_MIN = 1405; +const int ACCEL1_PEDAL_MIN = 2646; +const int ACCEL2_PEDAL_MIN = 1030; -const int BRAKE1_PEDAL_MAX = 1945; -const int BRAKE2_PEDAL_MAX = 1742; +const int BRAKE1_PEDAL_MAX = 2022; +const int BRAKE2_PEDAL_MAX = 1668; const int BRAKE1_PEDAL_MIN = 1192; const int BRAKE2_PEDAL_MIN = 2476; diff --git a/lib/interfaces/include/DrivebrainETHInterface.h b/lib/interfaces/include/DrivebrainETHInterface.h new file mode 100644 index 000000000..e3a3a0ee2 --- /dev/null +++ b/lib/interfaces/include/DrivebrainETHInterface.h @@ -0,0 +1,24 @@ +#ifndef __DRIVEBRAINETHINTERFACE_H__ +#define __DRIVEBRAINETHINTERFACE_H__ +#include "hytech_msgs.pb.h" +#include "DrivebrainData.h" +#include "SharedDataTypes.h" +class DrivebrainETHInterface +{ +public: + DrivebrainETHInterface(){ + _latest_data.last_receive_time_millis = -1; + _latest_data.DB_prev_MCU_recv_millis = -1; + }; + + void receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis); + + hytech_msgs_MCUOutputData make_db_msg(const SharedCarState_s &shared_state); + DrivebrainData_s get_latest_data() { return _latest_data; } + +private: + + DrivebrainData_s _latest_data = {}; +}; + +#endif // __DRIVEBRAINETHINTERFACE_H__ \ No newline at end of file diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 77a250cc1..b2c2249fa 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -166,7 +166,6 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_YPR_CANID: interfaces.vn_interface->retrieve_ypr_CAN(recvd_msg); break; - case VN_LAT_LON_CANID: interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg); break; @@ -179,6 +178,8 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_ANGULAR_RATE_CANID: interfaces.vn_interface->receive_ang_rates_CAN(recvd_msg); break; + default: + break; } } } diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index be9652dae..41312a1c1 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -119,8 +119,8 @@ class InverterInterface } int16_t get_speed() { return speed_; } - float get_torque_current() { return torque_current_; } - float get_mag_current() { return magnetizing_current_; } + float get_motor_torque() { return actual_motor_torque_; } + int16_t get_commanded_torque() { return commanded_torque_; } float get_actual_torque() { return actual_torque_nm_; } uint16_t get_error_status(); MC_temps get_temps_msg() { return mc_temps_; } @@ -128,7 +128,7 @@ class InverterInterface private: float id110_val_; // for scaling to proper iq and id vals - float torque_current_, magnetizing_current_; // iq and id in A respectively + int16_t actual_motor_torque_, commanded_torque_; float actual_torque_nm_; /* write setpoints message to the CAN buffer */ void write_cmd_msg_to_queue_(MC_setpoints_command msg); diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index 7dd835457..12aa71274 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -131,9 +131,10 @@ void InverterInterface::receive_status_msg(CAN_message_t &msg) // https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf#page=83&zoom=100,76,82 // for the actual conversion. requires looking at the current params of the inverter // to get scalar for this. - // torque_current_ = ((float)mc_status.get_torque_current() * id110_val_) / 16384.0; // iq - torque_current_ = mc_status.get_torque_current(); - magnetizing_current_ = ((float)mc_status.get_magnetizing_current() * id110_val_) / 16384.0; // id + // actual_motor_torque_ = ((float)mc_status.get_motor_torque() * id110_val_) / 16384.0; // iq + actual_motor_torque_ = mc_status.get_torque_current(); + commanded_torque_ = mc_status.get_magnetizing_current(); // id + // TODO enable this on the inverters // actual torque in Nm is from the signal we can add in from here: diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index 7d5dc4d0c..f0e260f36 100644 --- a/lib/interfaces/include/LoadCellInterface.h +++ b/lib/interfaces/include/LoadCellInterface.h @@ -19,6 +19,14 @@ struct LoadCellInterfaceTick_s class LoadCellInterface { +public: + LoadCellInterface() { + loadCellConversions_ = veh_vec(); + loadCellForcesUnfiltered_ = veh_vec(); + loadCellForcesFiltered_ = veh_vec(); + } + void tick(const LoadCellInterfaceTick_s &intake); + LoadCellInterfaceOutput_s getLoadCellForces(); private: /* FIR filter designed with @@ -50,10 +58,7 @@ class LoadCellInterface veh_vec loadCellForcesUnfiltered_; veh_vec loadCellForcesFiltered_; bool FIRSaturated_ = false; -public: - LoadCellInterface() {} - void tick(const LoadCellInterfaceTick_s &intake); - LoadCellInterfaceOutput_s getLoadCellForces(); + }; #endif \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index 05e660442..3f874bc29 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -5,39 +5,36 @@ #include "pb_decode.h" #include "pb_common.h" #include "circular_buffer.h" -#include "NativeEthernet.h" -#include "MCU_rev15_defs.h" -#include "InterfaceParams.h" +#include + +#include struct ETHInterfaces { }; -using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); - // this should be usable with arbitrary functions idk something -void handle_ethernet_socket_receive(EthernetUDP* socket, recv_function_t recv_function, ETHInterfaces& interfaces) +template +void handle_ethernet_socket_receive(const SysTick_s& tick, qindesign::network::EthernetUDP *socket, std::function recv_function, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { int packet_size = socket->parsePacket(); - if(packet_size > 0) - { - Serial.println("packet size"); - Serial.println(packet_size); - uint8_t buffer[EthParams::default_buffer_size]; + if (packet_size > 0) + { + uint8_t buffer[buffer_size]; size_t read_bytes = socket->read(buffer, sizeof(buffer)); - socket->read(buffer, UDP_TX_PACKET_MAX_SIZE); - recv_function(buffer, read_bytes, interfaces); + socket->read(buffer, buffer_size); + recv_function(tick.millis, buffer, read_bytes, interface, desc_pointer); } } -template -bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, const pb_msgdesc_t* msg_desc) +template +bool handle_ethernet_socket_send_pb(IPAddress addr, uint16_t port, qindesign::network::EthernetUDP *socket, const pb_struct &msg, const pb_msgdesc_t *msg_desc) { - socket->beginPacket(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port); - - uint8_t buffer[EthParams::default_buffer_size]; + socket->beginPacket(addr, port); + uint8_t buffer[buffer_size]; pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); - if (!pb_encode(&stream, msg_desc, &msg)) { + if (!pb_encode(&stream, msg_desc, &msg)) + { // You can handle error more explicitly by looking at stream.errmsg return false; } @@ -47,19 +44,15 @@ bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, c return true; } - -template -std::pair recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces, const pb_msgdesc_t * desc_pointer) +template +void recv_pb_stream_msg(unsigned long curr_millis, const uint8_t *buffer, size_t packet_size, eth_interface &interface, const pb_msgdesc_t *desc_pointer) { pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); pb_msg_type msg = {}; if (pb_decode(&stream, desc_pointer, &msg)) { - return {msg, true}; + interface.receive_pb_msg(msg, curr_millis); } - return {msg, }; } - - #endif \ No newline at end of file diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index e8e028837..1b8a11a2e 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -100,7 +100,14 @@ class TelemetryInterface InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); + void update_drivetrain_torque_filter_out_telem_CAN_msg( + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr + ); void update_penthouse_accum_CAN_msg( const AnalogConversion_s ¤t, const AnalogConversion_s &reference); diff --git a/lib/interfaces/library.json b/lib/interfaces/library.json index 202cee41a..359a0f9e7 100644 --- a/lib/interfaces/library.json +++ b/lib/interfaces/library.json @@ -5,7 +5,8 @@ "dependencies": { "can_lib": "*", "shared_data": "*", - "CASE_lib": "*" + "CASE_lib": "*", + "nanopb": "*" }, "frameworks": "*", "platforms": "*" diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp new file mode 100644 index 000000000..29aadf493 --- /dev/null +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -0,0 +1,31 @@ +#include "DrivebrainETHInterface.h" +#include "SharedDataTypes.h" +#include + +hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarState_s &shared_state) +{ + hytech_msgs_MCUOutputData out; + out.accel_percent = shared_state.pedals_data.accelPercent; + out.brake_percent = shared_state.pedals_data.brakePercent; + out.rpm_data = {shared_state.drivetrain_data.measuredSpeeds[0], + shared_state.drivetrain_data.measuredSpeeds[1], + shared_state.drivetrain_data.measuredSpeeds[2], + shared_state.drivetrain_data.measuredSpeeds[3]}; + + out.steering_angle_deg = shared_state.steering_data.angle; + out.MCU_recv_millis = _latest_data.last_receive_time_millis; + return out; +} + +void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &msg_in, unsigned long curr_millis) +{ + veh_vec nm_lim(msg_in.torque_limit_nm.FL, msg_in.torque_limit_nm.FR, msg_in.torque_limit_nm.RL, msg_in.torque_limit_nm.RR); + + veh_vec speed_set(msg_in.desired_rpms.FL, msg_in.desired_rpms.FR, msg_in.desired_rpms.RL, msg_in.desired_rpms.RR); + + _latest_data.torque_limits_nm = nm_lim; + _latest_data.speed_setpoints_rpm = speed_set; + _latest_data.DB_prev_MCU_recv_millis = msg_in.prev_MCU_recv_millis; + _latest_data.last_receive_time_millis = curr_millis; // current tick millis + +} \ No newline at end of file diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index e96a0b6cf..15b5f55a1 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -167,14 +167,31 @@ 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 = 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(); + torque.fl_motor_torque = fl->get_motor_torque(); + torque.fr_motor_torque = fr->get_motor_torque(); + torque.rl_motor_torque = rl->get_motor_torque(); + torque.rr_motor_torque = rr->get_motor_torque(); enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech); } +void TelemetryInterface::update_drivetrain_torque_filter_out_telem_CAN_msg( + 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 + DRIVETRAIN_FILTER_OUT_TORQUE_TEL_t torque; + torque.fl_motor_torque = fl->get_commanded_torque(); + torque.fr_motor_torque = fr->get_commanded_torque(); + torque.rl_motor_torque = rl->get_commanded_torque(); + torque.rr_motor_torque = rr->get_commanded_torque(); + + enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_FILTER_OUT_TORQUE_TEL_hytech); +} + // Pack_PENTHOUSE_ACCUM_MSG_hytech void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s ¤t, const AnalogConversion_s &reference) { @@ -281,6 +298,7 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, update_drivetrain_err_status_CAN_msg(fl, fr, rl, rr); update_drivetrain_status_telem_CAN_msg(fl, fr, rl, rr, accel_implaus, brake_implaus, accel_per, brake_per); update_drivetrain_torque_telem_CAN_msg(fl, fr, rl, rr); + update_drivetrain_torque_filter_out_telem_CAN_msg(fl, fr, rl, rr); update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel], adc1.conversions[channels_.current_ref_channel]); diff --git a/lib/mock_interfaces/DrivebrainInterface.h b/lib/mock_interfaces/DrivebrainInterface.h new file mode 100644 index 000000000..ba6fb5ca0 --- /dev/null +++ b/lib/mock_interfaces/DrivebrainInterface.h @@ -0,0 +1,5 @@ +#ifndef __DRIVEBRAININTERFACE_H__ +#define __DRIVEBRAININTERFACE_H__ + + +#endif // __DRIVEBRAININTERFACE_H__ \ No newline at end of file diff --git a/lib/shared_data/include/DrivebrainData.h b/lib/shared_data/include/DrivebrainData.h new file mode 100644 index 000000000..35cada9bf --- /dev/null +++ b/lib/shared_data/include/DrivebrainData.h @@ -0,0 +1,16 @@ +#ifndef __DRIVEBRAINDATA_H__ +#define __DRIVEBRAINDATA_H__ + +#include "Utility.h" + +struct DrivebrainData_s +{ + /// @brief the latest time that the MCU received a message w.r.t the current tick's millis + int64_t last_receive_time_millis = -1; + /// @brief the latest MCU last_receive_time_millis that the drivebrain received + int64_t DB_prev_MCU_recv_millis = -1; + veh_vec torque_limits_nm; + veh_vec speed_setpoints_rpm; +}; + +#endif // __DRIVEBRAINDATA_H__ \ No newline at end of file diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index e8978c772..8de8f1a45 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -4,6 +4,10 @@ #include "Utility.h" #include "SysClock.h" #include "SharedFirmwareTypes.h" +#include "DrivebrainData.h" + +using speed_rpm = float; +using torque_nm = float; /// @brief Defines modes of torque limit to be processed in torque limit map for exact values. enum class TorqueLimit_e @@ -45,8 +49,8 @@ struct PedalsSystemData_s struct DrivetrainDynamicReport_s { uint16_t measuredInverterFLPackVoltage; - float measuredSpeeds[NUM_MOTORS]; - float measuredTorques[NUM_MOTORS]; + speed_rpm measuredSpeeds[NUM_MOTORS]; // rpm + torque_nm measuredTorques[NUM_MOTORS]; float measuredTorqueCurrents[NUM_MOTORS]; float measuredMagnetizingCurrents[NUM_MOTORS]; }; @@ -145,19 +149,25 @@ struct SharedCarState_s LoadCellInterfaceOutput_s loadcell_data; PedalsSystemData_s pedals_data; VectornavData_s vn_data; + DrivebrainData_s db_data; + TorqueControllerMuxStatus tc_mux_status; 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 VectornavData_s &_vn_data) + const VectornavData_s &_vn_data, + const DrivebrainData_s &_db_data, + const TorqueControllerMuxStatus &_tc_mux_status) : systick(_systick), steering_data(_steering_data), drivetrain_data(_drivetrain_data), loadcell_data(_loadcell_data), pedals_data(_pedals_data), - vn_data(_vn_data) + vn_data(_vn_data), + db_data(_db_data), + tc_mux_status(_tc_mux_status) { // constructor body (if needed) } diff --git a/lib/shared_data/include/Utility.h b/lib/shared_data/include/Utility.h index 820d0f4a4..d528a73f6 100644 --- a/lib/shared_data/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -1,12 +1,13 @@ #ifndef __UTILITY_H__ #define __UTILITY_H__ #include "PhysicalParameters.h" +#include // Defines -const int FL = 0; -const int FR = 1; -const int RL = 2; -const int RR = 3; -const int NUM_MOTORS = 4; +const int FL = 0; +const int FR = 1; +const int RL = 2; +const int RR = 3; +const int NUM_MOTORS = 4; const int DEBOUNCE_MILLIS = 10; // milliseconds before registering another button press @@ -14,10 +15,30 @@ const int DEBOUNCE_MILLIS = 10; // milliseconds before registering another butto template struct veh_vec { +public: T FL; T FR; T RL; T RR; + +public: + veh_vec() = default; + veh_vec(T _FL, T _FR, T _RL, T _RR) + { + FL = _FL; + FR = _FR; + RL = _RL; + RR = _RR; + } + + /// @brief copy values to array in FL, FR, RL, RR order + void copy_to_arr(T (&arr_out)[4]) + { + arr_out[0] = FL; + arr_out[1] = FR; + arr_out[2] = RL; + arr_out[3] = RR; + } }; template diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index d142d4bdb..791511dbe 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -11,7 +11,6 @@ #include "SafetySystem.h" #include "DashboardInterface.h" #include "AMSInterface.h" -#include "PrintLogger.h" enum class CAR_STATE { @@ -74,7 +73,6 @@ 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 36be0afa0..8958c7b09 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -13,41 +13,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: { - // Serial.println("eror in ts not active:"); - // Serial.println(safety_system_->get_software_is_ok()); - // Serial.println(); - // hal_println("tractive system not active state"); - - // auto data = pedals_->getPedalsSystemData(); - // auto mux_test = controller_mux_->getDrivetrainCommand(); - - // hal_println("speeds 1 through 4"); - // Serial.println(mux_test.speeds_rpm[0]); - // Serial.println(mux_test.speeds_rpm[1]); - // Serial.println(mux_test.speeds_rpm[2]); - // Serial.println(mux_test.speeds_rpm[3]); - - // hal_println("torqeus"); - // Serial.println(mux_test.torqueSetpoints[0]); - // Serial.println(mux_test.torqueSetpoints[1]); - // Serial.println(mux_test.torqueSetpoints[2]); - // Serial.println(mux_test.torqueSetpoints[3]); - // Serial.println(); - // Serial.print(data.brakeImplausible); - // Serial.print(" "); - // Serial.print(data.accelImplausible); - // Serial.print(" "); - // Serial.print(data.brakeAndAccelPressedImplausibility); - // Serial.print(" "); - - // Serial.println("accel, brake:"); - // Serial.print(data.accelPercent); - // Serial.print(" "); - // Serial.print(data.brakePercent); - // Serial.print(" \n"); - // 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()) @@ -63,7 +28,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren { buzzer_->deactivate(); } - // hal_println("in tractive system active state"); // TODO migrate to new pedals system auto data = pedals_->getPedalsSystemData(); drivetrain_->disable_no_pins(); @@ -83,7 +47,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::ENABLING_INVERTERS: { - // hal_println("in enabling inverters state"); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); @@ -144,8 +107,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren 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 @@ -161,14 +122,11 @@ void MCUStateMachine::tick_state_machine(unsigned long curren template void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) { - // hal_println("running exit logic"); - // hal_printf("\n to state: %i\n", new_state); handle_exit_logic_(current_state_, curr_time); current_state_ = new_state; - // hal_println("running entry logic"); handle_entry_logic_(new_state, curr_time); } @@ -215,13 +173,10 @@ void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state { // make dashboard sound buzzer buzzer_->activate_buzzer(curr_time); - // hal_println("RTDS enabled"); break; } case CAR_STATE::READY_TO_DRIVE: { - // hal_printf("%i\n", curr_time); - // hal_println("Ready to drive"); break; } } diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h new file mode 100644 index 000000000..7eb7b6e48 --- /dev/null +++ b/lib/systems/include/DrivebrainController.h @@ -0,0 +1,47 @@ +#ifndef __DRIVEBRAINCONTROLLER_H__ +#define __DRIVEBRAINCONTROLLER_H__ + + +#include "TorqueControllers.h" +#include "DrivebrainData.h" +#include "BaseController.h" +#include "SharedDataTypes.h" + +// TODO - [x] need to validate that the times that are apparent in the drivebrain data +// and ensure that they are within tolerence to current sys-tick + +// TODO - [ ] if the drivebrain controller is currently the active controller, +// the latency fault should be latching + +// meaning that if we fail any of the latency checks while active we cannot clear the timing failure fault +// unless we switch to another controller and back again. in reality, the CAN line or ethernet conditions +// probably wont improve randomly during runtime so it will keep faulting. primarily this is just to make sure +// we dont keep going from latent to not latent while driving and thus the driver gets jittered and the +// drivetrain will keep "powering up" and "powering down" + +class DrivebrainController : public Controller +{ +public: + + DrivebrainController(unsigned long allowed_latency, + float max_fault_clear_speed_m_s = 1.0, + ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4) + { + _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode}; + } + + TorqueControllerOutput_s evaluate(const SharedCarState_s &state); + +private: + struct + { + unsigned long allowed_latency; + float max_fault_clear_speed_m_s; + ControllerMode_e assigned_controller_mode; + } _params; + + bool _timing_failure = false; + unsigned long _last_setpoint_millis = -1; +}; + +#endif // __DRIVEBRAINCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index 5c2b52356..87eb3085f 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -233,8 +233,8 @@ DrivetrainDynamicReport_s DrivetrainSystem::get_dynamic_data() int inverter_ind = 0; for (auto inv_pointer : inverters_) { - auto iq = inv_pointer->get_torque_current(); // iq in A - auto id = inv_pointer->get_mag_current(); // id in A + auto iq = inv_pointer->get_motor_torque(); // iq in A + auto id = inv_pointer->get_commanded_torque(); // id in A dynamic_data_.measuredSpeeds[inverter_ind] = inv_pointer->get_speed(); dynamic_data_.measuredTorqueCurrents[inverter_ind] = iq; dynamic_data_.measuredMagnetizingCurrents[inverter_ind] = id; diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 30f646f4f..ed8eaa829 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -1,6 +1,7 @@ #ifndef TORQUECONTROLLERMUX #define TORQUECONTROLLERMUX +#include #include #include "SharedDataTypes.h" #include "BaseController.h" diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp new file mode 100644 index 000000000..675c35178 --- /dev/null +++ b/lib/systems/src/DrivebrainController.cpp @@ -0,0 +1,49 @@ +#include "DrivebrainController.h" + + +TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state) +{ + + auto sys_tick = state.systick; + auto db_input = state.db_data; + + // cases for timing_failure: + // 1 if the last_receive_time_millis < 0, then we have not received any messages from the db (initialized at -1 in constructor) + bool no_messages_received = db_input.last_receive_time_millis < 0; + + // 2 if the DB_prev_MCU_recv_millis < 0, then the drivebrain has not received a time from the MCU + // (meaning that the MCU is not sending properly or the drivebrain is not receiving properly or it has + // yet to receive from the MCU yet) + bool drivebrain_has_not_received_time = (db_input.DB_prev_MCU_recv_millis < 0); + + // 3 if the time between the current MCU sys_tick.millis time and the last millis time that the drivebrain received is too high + bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.DB_prev_MCU_recv_millis)) > (int)_params.allowed_latency); + + + bool timing_failure = (message_too_latent || no_messages_received || drivebrain_has_not_received_time); + + // only in the case that our speed is low enough (<1 m/s) do we want to clear the fault + + bool is_active_controller = state.tc_mux_status.active_controller_mode == _params.assigned_controller_mode; + + if ((!is_active_controller) && (!timing_failure)) + { + // timing failure should be false here + _timing_failure = false; + } + + TorqueControllerOutput_s output; + if (!timing_failure && (!_timing_failure)) + { + _last_setpoint_millis = db_input.last_receive_time_millis; + + db_input.speed_setpoints_rpm.copy_to_arr(output.command.speeds_rpm); + db_input.torque_limits_nm.copy_to_arr(output.command.inverter_torque_limit); + } + else + { + _timing_failure = true; + output.command = {{0.0f}, {0.0f}}; + } + return output; +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 7ff412f6f..4d816a994 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,7 +4,8 @@ lib_deps_shared = git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 https://github.com/hytech-racing/shared_firmware_types.git#feb3b83 https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - + https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T04_27_55/hytech_msgs_pb_lib.tar.gz + [env:test_env] platform = native test_framework = googletest @@ -12,7 +13,11 @@ build_src_filter = -<**/*.c> -<**/*.cpp> build_unflags = -std=gnu++11 -build_flags = -std=c++17 -g + +build_flags = + -std=c++17 + -g + -D TESTING_SYSTEMS lib_ignore = interfaces-lib test_ignore= @@ -38,6 +43,7 @@ build_unflags = -std=gnu++11 build_flags = -std=c++17 -D TEENSY_OPT_SMALLEST_CODE + ; -D ENABLE_DEBUG_PRINTS check_tool = clangtidy check_src_filters = + @@ -53,6 +59,7 @@ upload_protocol = teensy-cli extra_scripts = pre:extra_script.py lib_deps = ${common.lib_deps_shared} + Nanopb SPI https://github.com/hytech-racing/shared_firmware_interfaces.git#shared_types https://github.com/tonton81/FlexCAN_T4 @@ -74,7 +81,6 @@ lib_ldf_mode = deep+ lib_deps = SPI Nanopb - https://github.com/vjmuzik/NativeEthernet.git https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#4ffbba4 https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz diff --git a/src/main.cpp b/src/main.cpp index 0f7c2e608..53552b3f6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,3 +1,9 @@ +#ifdef ENABLE_DEBUG_PRINTS +#define DEBUG_PRINTS true +#else +#define DEBUG_PRINTS false +#endif + /* Include files */ /* System Includes*/ #include @@ -5,10 +11,12 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" +#include "pb.h" #include "PrintLogger.h" // /* Interfaces */ - +#include "DrivebrainETHInterface.h" +#include "ProtobufMsgInterface.h" #include "HytechCANInterface.h" #include "ThermistorInterface.h" #include "Teensy_ADC.h" @@ -30,6 +38,7 @@ #include "SafetySystem.h" #include "DrivetrainSystem.h" #include "PedalsSystem.h" +#include "DrivebrainController.h" #include "TorqueControllerMux.h" #include "TorqueControllers.h" #include "CASESystem.h" @@ -38,9 +47,15 @@ #include "MCUStateMachine.h" #include "HT08_CASE.h" +#include "InterfaceParams.h" +#include "PrintLogger.h" + +#include "hytech_msgs.pb.h" /* PARAMETER STRUCTS */ +using namespace qindesign::network; + const TelemetryInterfaceReadChannels telem_read_channels = { .accel1_channel = MCU15_ACCEL1_CHANNEL, @@ -86,7 +101,6 @@ const PedalsParams accel2_only_params = { .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE}; - const PedalsParams accel_params = { .min_pedal_1 = ACCEL1_PEDAL_MIN, .min_pedal_2 = ACCEL2_PEDAL_MIN, @@ -101,7 +115,6 @@ const PedalsParams accel_params = { .implausibility_margin = DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN, .mechanical_activation_percentage = APPS_ACTIVATION_PERCENTAGE}; - const PedalsParams brake1_only_params = { .min_pedal_1 = BRAKE1_PEDAL_MIN, .min_pedal_2 = BRAKE1_PEDAL_MIN, @@ -251,7 +264,7 @@ CASEConfiguration case_config = { .useTorqueBias = true, .DriveTorquePercentFront = 0.5, // DON'T TOUCH UNTIL LOAD CELL ADHERES TO DRIVE BIAS .BrakeTorquePercentFront = 0.6, - .MechPowerMaxkW =51.5, // kW + .MechPowerMaxkW = 51.5, // kW .launchLeftRightMaxDiff = 2.0, // N-m .tcs_pid_lower_rpm_front = 0.0, // RPM .tcs_pid_upper_rpm_front = 5000.0, // RPM @@ -264,7 +277,7 @@ CASEConfiguration case_config = { .TCSGenLeftRightDiffUpperBound = 20, // N-m .TCSWheelSteerLowerBound = 2, // Deg .TCSWheelSteerUpperBound = 25, // Deg - .useRPM_TCS_GainSchedule = true, // If both are false, then P values defaults to lower bound per axle + .useRPM_TCS_GainSchedule = true, // If both are false, then P values defaults to lower bound per axle .useNL_TCS_GainSchedule = false, .TCS_NL_startBoundPerc_FrontAxle = 0.5, .TCS_NL_endBoundPerc_FrontAxle = 0.4, @@ -302,13 +315,14 @@ TorqueControllerCASEWrapper case_wrapper(&case_system); // mode 3 TorqueControllerSimpleLaunch simple_launch; // mode 4 -TorqueControllerSlipLaunch slip_launch; +DrivebrainController db_controller(210, 210); + TCMuxType 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}); + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&db_controller)}, + {false, false, true, false, true}); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); @@ -317,6 +331,7 @@ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system // GROUPING STRUCTS (To limit parameter count in utilizing functions) // */ +DrivebrainETHInterface db_eth_interface; CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &vn_interface, &dashboard, &ams_interface, &sab_interface}; /* @@ -332,6 +347,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); +void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg); /* SETUP @@ -342,9 +358,9 @@ void setup() // initialize CAN communication init_all_CAN_devices(); - // Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); - // protobuf_send_socket.begin(EthParams::default_protobuf_send_port); - // protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); + Ethernet.begin(EthParams::default_MCU_MAC_address, EthParams::default_MCU_ip); + protobuf_send_socket.begin(EthParams::default_protobuf_send_port); + protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); SPI.begin(); a1.init(); @@ -379,9 +395,9 @@ void setup() Init Interfaces */ - main_ecu.init(); // initial shutdown circuit readings, - wd_interface.init(curr_tick.millis); // initialize wd kick time - ams_interface.init(curr_tick); // initialize last heartbeat time + main_ecu.init(); // initial shutdown circuit readings, + wd_interface.init(curr_tick.millis); // initialize wd kick time + ams_interface.init(curr_tick); // initialize last heartbeat time steering1.init(); steering1.setOffset(PRIMARY_STEERING_SENSE_OFFSET); @@ -418,15 +434,19 @@ void loop() // single source of truth for the state of the car. // no systems or interfaces should write directly to this. SharedCarState_s car_state_inst(curr_tick, - steering_system.getSteeringSystemData(), - drivetrain.get_dynamic_data(), - load_cell_interface.getLoadCellForces(), - pedals_system.getPedalsSystemData(), - vn_interface.get_vn_struct()); + steering_system.getSteeringSystemData(), + drivetrain.get_dynamic_data(), + load_cell_interface.getLoadCellForces(), + pedals_system.getPedalsSystemData(), + vn_interface.get_vn_struct(), + db_eth_interface.get_latest_data(), + torque_controller_mux.get_tc_mux_status()); + + hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst); + + handle_ethernet_interface_comms(curr_tick, out_eth_msg); tick_all_systems(curr_tick); - - // logger.log_out(static_cast(torque_controller_mux.get_tc_mux_status().active_controller_mode), curr_tick.millis, 100); // inverter procedure before entering state machine // reset inverters if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) @@ -445,7 +465,8 @@ void loop() send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); // Basic debug prints - if (curr_tick.triggers.trigger5) + // if (curr_tick.triggers.trigger5) + if (DEBUG_PRINTS) { Serial.print("Steering system reported angle (deg): "); Serial.println(steering_system.getSteeringSystemData().angle); @@ -485,9 +506,11 @@ void loop() Serial.print("Current TC error: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_error)); Serial.println(); + Serial.print("dial state: "); + Serial.println(static_cast(dashboard.getDialMode())); + Serial.println(); Serial.println(); } - } /* @@ -548,6 +571,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy(); front_thermistors_interface.tick(mcu_adc.get().conversions[MCU15_THERM_FL_CHANNEL], mcu_adc.get().conversions[MCU15_THERM_FR_CHANNEL]); + // TODO pass in the shared state instead telem_interface.tick( a1.get(), a2.get(), @@ -567,12 +591,13 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE2_CHANNEL], pedals_system.getMechBrakeActiveThreshold(), torque_controller_mux.get_tc_mux_status().active_error); + + ams_interface.tick(current_system_tick); } if (t.trigger50) // 50Hz { steering1.sample(); - ams_interface.tick(current_system_tick); } if (t.trigger100) // 100Hz @@ -609,47 +634,12 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE1_CHANNEL], a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // accel 1 only accel 2 dead, brake normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // accel 2 only accel 1 dead, brake normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // brake 1 only brake 2 dead, accel normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL], - // a1.get().conversions[MCU15_BRAKE1_CHANNEL]); - // brake 2 only brake 1 dead, accel normal - // pedals_system.tick( - // current_system_tick, - // a1.get().conversions[MCU15_ACCEL1_CHANNEL], - // a1.get().conversions[MCU15_ACCEL2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL], - // a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - - // tick steering system steering_system.tick( (SteeringSystemTick_s){ .tick = current_system_tick, .secondaryConversion = a1.get().conversions[MCU15_STEERING_CHANNEL]}); - // Serial.println("Steering angle"); - // Serial.println(steering_system.getSteeringSystemData().angle); - // Serial.println("Steering status"); - // Serial.println(static_cast(steering_system.getSteeringSystemData().status)); - // tick drivetrain system drivetrain.tick(current_system_tick); // // tick torque controller mux @@ -667,3 +657,17 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) vn_interface.get_vn_struct().vn_status); } + +void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg) +{ + // 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. + + std::function recv_boi = &recv_pb_stream_msg; + handle_ethernet_socket_receive<1024, hytech_msgs_MCUCommandData>(systick, &protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUCommandData_fields); + + if(systick.triggers.trigger500) + { + handle_ethernet_socket_send_pb(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields); + } +} diff --git a/test/test_systems/drivebrain_controller_test.h b/test/test_systems/drivebrain_controller_test.h new file mode 100644 index 000000000..defffe451 --- /dev/null +++ b/test/test_systems/drivebrain_controller_test.h @@ -0,0 +1,63 @@ +#ifndef DRIVEBRAIN_CONTROLLER_TEST +#define DRIVEBRAIN_CONTROLLER_TEST +#include +#include +#include + +auto runTick(DrivebrainController *controller, float last_DB_receive_time_millis, float last_receive_time_millis, ControllerMode_e current_control_mode, unsigned long systick_millis) +{ + DrivebrainData_s data; + data.last_receive_time_millis = last_receive_time_millis; + data.DB_prev_MCU_recv_millis = last_DB_receive_time_millis; + data.torque_limits_nm = {1, 1, 1, 1}; + data.speed_setpoints_rpm = {1, 1, 1, 1}; + + SysTick_s systick; + systick.millis = systick_millis; + systick.micros = 1000; + + TorqueControllerMuxStatus status = {}; + status.active_controller_mode = current_control_mode; + SharedCarState_s state(systick, {}, {}, {}, {}, {}, data, status); + return controller->evaluate(state); +} + +TEST(DrivebrainControllerTesting, signals_sent_within_range) +{ + DrivebrainController controller(10); + auto torque_controller_output_s = runTick(&controller, 998, 1001, ControllerMode_e::MODE_4, 1002); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +TEST(DrivebrainControllerTesting, setpoint_too_latent) +{ + DrivebrainController controller(5); + auto torque_controller_output_s = runTick(&controller, 800, 1006, ControllerMode_e::MODE_4, 1012); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); +} + +TEST(DrivebrainControllerTesting, failing_stay_failing) +{ + DrivebrainController controller(10); + auto torque_controller_output_s = runTick(&controller, 200, 1011, ControllerMode_e::MODE_4, 1032); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + + torque_controller_output_s = runTick(&controller, 200, 1033, ControllerMode_e::MODE_4, 1033); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + + torque_controller_output_s = runTick(&controller, 400, 1034, ControllerMode_e::MODE_4, 1034); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); +} + +TEST(DrivebrainControllerTesting, failing_reset_success) +{ + DrivebrainController controller(10); + auto torque_controller_output_s = runTick(&controller, 300, 1011, ControllerMode_e::MODE_4, 1022); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 0); + + torque_controller_output_s = runTick(&controller, 1020, 1021, ControllerMode_e::MODE_3, 1023); + torque_controller_output_s = runTick(&controller, 1022, 1022, ControllerMode_e::MODE_4, 1024); + EXPECT_FLOAT_EQ(torque_controller_output_s.command.speeds_rpm[0], 1); +} + +#endif \ 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 2004ead69..7f8c4f871 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); - 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, {}); + 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}); - 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 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); - SharedCarState_s 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); - SharedCarState_s 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); - SharedCarState_s 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 - SharedCarState_s 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 - SharedCarState_s 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/main.cpp b/test/test_systems/main.cpp index 75d2b93a8..fb591ee44 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 "drivebrain_controller_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/state_machine_test.h b/test/test_systems/state_machine_test.h index 5c17eece3..0c4e8c909 100644 --- a/test/test_systems/state_machine_test.h +++ b/test/test_systems/state_machine_test.h @@ -31,7 +31,7 @@ class DrivetrainMock void disable_no_pins(){}; }; -SharedCarState_s dummy_state({}, {}, {}, {}, {}, {}); +SharedCarState_s dummy_state({}, {}, {}, {}, {}, {}, {}, {}); DumbController c; 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 5347a6745..6e6a3cf29 100644 --- a/test/test_systems/tc_mux_v2_testing.h +++ b/test/test_systems/tc_mux_v2_testing.h @@ -53,7 +53,7 @@ TEST(TorqueControllerMuxTesting, test_invalid_controller_request_error) { TestControllerType inst1, inst2; TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); - SharedCarState_s 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().active_error, TorqueControllerMuxError::ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS); @@ -74,7 +74,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}); - SharedCarState_s state({}, {}, {}, {}, {}, {}); + SharedCarState_s state({}, {}, {}, {}, {}, {}, {}, {}); set_four_outputs(state.drivetrain_data.measuredSpeeds, 10000.0); state.pedals_data = {}; @@ -104,7 +104,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}); - SharedCarState_s 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); @@ -177,7 +177,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) static_cast(&simple_launch), static_cast(&slip_launch)}, {false, true, false, false, false}); - SharedCarState_s 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.inverter_torque_limit[0], (max_torque / 2), 0.01); @@ -185,7 +185,7 @@ TEST(TorqueControllerMuxTesting, test_mode0_evaluation) ASSERT_NEAR(out.inverter_torque_limit[2], (max_torque / 2), 0.01); ASSERT_NEAR(out.inverter_torque_limit[3], (max_torque / 2), 0.01); - mode_0_input_state = {{}, {}, {}, {}, {.accelPercent = 0.0f, .brakePercent = 0.0f, .regenPercent = 0.0}, {}}; + 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.inverter_torque_limit[0], 0); ASSERT_EQ(out.inverter_torque_limit[1], 0); @@ -206,7 +206,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) { drivetrain_data.measuredSpeeds[i] = 500.0f; } - SharedCarState_s 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); @@ -220,7 +220,7 @@ TEST(TorqueControllerMuxTesting, test_power_limit) } set_output_rpm(inst1, 20000, 21.0); - SharedCarState_s 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++) @@ -244,7 +244,7 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) drivetrain_data.measuredSpeeds[i] = 500.0f; } - SharedCarState_s 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); @@ -272,7 +272,7 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) TEST(TorqueControllerMuxTesting, test_null_pointer_error_state) { TorqueControllerMux<1> test({nullptr}, {true}); - SharedCarState_s state({}, {}, {}, {}, {}, {}); + SharedCarState_s state({}, {}, {}, {}, {}, {}, {}, {}); auto res = test.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_LOW_TORQUE, state); for (int i = 0; i < 4; i++) { From 10775c6d2646cf8a29b1351bf32fc579e857be64 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 20 Oct 2024 16:29:12 -0400 Subject: [PATCH 28/28] Feature/drivebrain interface load cell and timing (#113) * tuning pedals and updated to latest can definition * drivebrain controller creation * added receiving of drivebrain messages * fixing pedals tests * adding drivebrain interface empty mock header * feat(drivebrain_controller): added tests for drivebrain controller latency and jitter * adding in latching logic for the db controller * added in drivebrain interface into main * ensuring that the regen and torque limits dont get applied to the drivebrain controller * fix(drivebrain_controller): fixed drivebrain_controller failure and reset logic * adjusting logic; removing interaction with tc and giving drivebrain more responsbility * fixing oopsie in drivebrain control mode and adding it to main * ready to test on car * fixing header * seemingly working ethernet integration * switching ethernet driver * removing CAN based drivebrain interface, addressing naming for inverter interface updates, updating ethernet interface since only one message is received, and fixing tests for new interface * making the veh_vec constuctor actually a constructor * addressing comments * a little cleanup * added compiler macro flag for debug prints * added in type alias * adding in better handling of timing for drivebrain message latency * updated to latest HT-prot * updating pedal params and to latest HT_proto * added in loadcell data to the output message and added in more metrics for drivebrain comms * updating name and changing loadcell data to int * merge changes * fixing removal of eth interface comms --------- Co-authored-by: sreekara --- lib/interfaces/include/LoadCellInterface.h | 3 ++ lib/interfaces/src/DrivebrainETHInterface.cpp | 20 ++++++--- lib/interfaces/src/LoadCellInterface.cpp | 11 +++-- lib/shared_data/include/SharedDataTypes.h | 28 ++++++++++++ lib/systems/include/DrivebrainController.h | 4 ++ lib/systems/src/DrivebrainController.cpp | 17 ++++++-- platformio.ini | 2 +- src/main.cpp | 43 ++++++++++--------- 8 files changed, 93 insertions(+), 35 deletions(-) diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index f0e260f36..5cfd37947 100644 --- a/lib/interfaces/include/LoadCellInterface.h +++ b/lib/interfaces/include/LoadCellInterface.h @@ -26,6 +26,8 @@ class LoadCellInterface loadCellForcesFiltered_ = veh_vec(); } void tick(const LoadCellInterfaceTick_s &intake); + void update_raw_data(const LoadCellInterfaceTick_s &intake); + LoadCellInterfaceRawOutput_s get_latest_raw_data() { return _raw_data; } LoadCellInterfaceOutput_s getLoadCellForces(); private: /* @@ -57,6 +59,7 @@ class LoadCellInterface veh_vec loadCellConversions_; veh_vec loadCellForcesUnfiltered_; veh_vec loadCellForcesFiltered_; + LoadCellInterfaceRawOutput_s _raw_data; bool FIRSaturated_ = false; }; diff --git a/lib/interfaces/src/DrivebrainETHInterface.cpp b/lib/interfaces/src/DrivebrainETHInterface.cpp index 29aadf493..3626541c1 100644 --- a/lib/interfaces/src/DrivebrainETHInterface.cpp +++ b/lib/interfaces/src/DrivebrainETHInterface.cpp @@ -7,13 +7,20 @@ hytech_msgs_MCUOutputData DrivebrainETHInterface::make_db_msg(const SharedCarSta hytech_msgs_MCUOutputData out; out.accel_percent = shared_state.pedals_data.accelPercent; out.brake_percent = shared_state.pedals_data.brakePercent; - out.rpm_data = {shared_state.drivetrain_data.measuredSpeeds[0], - shared_state.drivetrain_data.measuredSpeeds[1], - shared_state.drivetrain_data.measuredSpeeds[2], - shared_state.drivetrain_data.measuredSpeeds[3]}; - + + out.has_rpm_data = true; + out.rpm_data.FL = shared_state.drivetrain_data.measuredSpeeds[0]; + out.rpm_data.FR = shared_state.drivetrain_data.measuredSpeeds[1]; + out.rpm_data.RL = shared_state.drivetrain_data.measuredSpeeds[2]; + out.rpm_data.RR = shared_state.drivetrain_data.measuredSpeeds[3]; + out.steering_angle_deg = shared_state.steering_data.angle; - out.MCU_recv_millis = _latest_data.last_receive_time_millis; + out.MCU_recv_millis = _latest_data.last_receive_time_millis; + out.load_cell_data = {shared_state.raw_loadcell_data.raw_load_cell_data.FL, + shared_state.raw_loadcell_data.raw_load_cell_data.FR, + shared_state.raw_loadcell_data.raw_load_cell_data.RL, + shared_state.raw_loadcell_data.raw_load_cell_data.RR}; + out.has_load_cell_data = true; return out; } @@ -27,5 +34,4 @@ void DrivebrainETHInterface::receive_pb_msg(const hytech_msgs_MCUCommandData &ms _latest_data.speed_setpoints_rpm = speed_set; _latest_data.DB_prev_MCU_recv_millis = msg_in.prev_MCU_recv_millis; _latest_data.last_receive_time_millis = curr_millis; // current tick millis - } \ No newline at end of file diff --git a/lib/interfaces/src/LoadCellInterface.cpp b/lib/interfaces/src/LoadCellInterface.cpp index b6aa570f3..b3a032ee0 100644 --- a/lib/interfaces/src/LoadCellInterface.cpp +++ b/lib/interfaces/src/LoadCellInterface.cpp @@ -34,9 +34,14 @@ void LoadCellInterface::tick(const LoadCellInterfaceTick_s &intake) LoadCellInterfaceOutput_s LoadCellInterface::getLoadCellForces() { - return (LoadCellInterfaceOutput_s) { + return (LoadCellInterfaceOutput_s){ .loadCellForcesFiltered = loadCellForcesFiltered_, .loadCellConversions = loadCellConversions_, - .FIRSaturated = FIRSaturated_ - }; + .FIRSaturated = FIRSaturated_}; +} + +// this is a hack, i know, i just want all the data. +void LoadCellInterface::update_raw_data(const LoadCellInterfaceTick_s &intake) +{ + _raw_data.raw_load_cell_data = {intake.FLConversion.raw, intake.FRConversion.raw, intake.RLConversion.raw, intake.RRConversion.raw}; } \ No newline at end of file diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h index 8de8f1a45..fc2b14928 100644 --- a/lib/shared_data/include/SharedDataTypes.h +++ b/lib/shared_data/include/SharedDataTypes.h @@ -120,6 +120,11 @@ struct LoadCellInterfaceOutput_s bool FIRSaturated; }; +struct LoadCellInterfaceRawOutput_s +{ + veh_vec raw_load_cell_data; +}; + // Enums enum class SteeringSystemStatus_e { @@ -147,6 +152,7 @@ struct SharedCarState_s SteeringSystemData_s steering_data; DrivetrainDynamicReport_s drivetrain_data; LoadCellInterfaceOutput_s loadcell_data; + LoadCellInterfaceRawOutput_s raw_loadcell_data; PedalsSystemData_s pedals_data; VectornavData_s vn_data; DrivebrainData_s db_data; @@ -164,6 +170,28 @@ struct SharedCarState_s steering_data(_steering_data), drivetrain_data(_drivetrain_data), loadcell_data(_loadcell_data), + raw_loadcell_data({}), + pedals_data(_pedals_data), + vn_data(_vn_data), + db_data(_db_data), + tc_mux_status(_tc_mux_status) + { + // constructor body (if needed) + } + SharedCarState_s(const SysTick_s &_systick, + const SteeringSystemData_s &_steering_data, + const DrivetrainDynamicReport_s &_drivetrain_data, + const LoadCellInterfaceOutput_s &_loadcell_data, + const LoadCellInterfaceRawOutput_s &_raw_loadcell_data, + const PedalsSystemData_s &_pedals_data, + const VectornavData_s &_vn_data, + const DrivebrainData_s &_db_data, + const TorqueControllerMuxStatus &_tc_mux_status) + : systick(_systick), + steering_data(_steering_data), + drivetrain_data(_drivetrain_data), + loadcell_data(_loadcell_data), + raw_loadcell_data(_raw_loadcell_data), pedals_data(_pedals_data), vn_data(_vn_data), db_data(_db_data), diff --git a/lib/systems/include/DrivebrainController.h b/lib/systems/include/DrivebrainController.h index 7eb7b6e48..ce3e0da5a 100644 --- a/lib/systems/include/DrivebrainController.h +++ b/lib/systems/include/DrivebrainController.h @@ -27,6 +27,8 @@ class DrivebrainController : public Controller float max_fault_clear_speed_m_s = 1.0, ControllerMode_e assigned_controller_mode = ControllerMode_e::MODE_4) { + _last_worst_latency_timestamp = 0; + _worst_latency_so_far = -1; _params = {allowed_latency, max_fault_clear_speed_m_s, assigned_controller_mode}; } @@ -40,6 +42,8 @@ class DrivebrainController : public Controller ControllerMode_e assigned_controller_mode; } _params; + unsigned long _last_worst_latency_timestamp; + int64_t _worst_latency_so_far; bool _timing_failure = false; unsigned long _last_setpoint_millis = -1; }; diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp index 675c35178..82278ba8d 100644 --- a/lib/systems/src/DrivebrainController.cpp +++ b/lib/systems/src/DrivebrainController.cpp @@ -1,6 +1,5 @@ #include "DrivebrainController.h" - TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state) { @@ -15,10 +14,22 @@ TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s & // (meaning that the MCU is not sending properly or the drivebrain is not receiving properly or it has // yet to receive from the MCU yet) bool drivebrain_has_not_received_time = (db_input.DB_prev_MCU_recv_millis < 0); - + // Serial.println("uh"); // 3 if the time between the current MCU sys_tick.millis time and the last millis time that the drivebrain received is too high bool message_too_latent = (::abs((int)(sys_tick.millis - db_input.DB_prev_MCU_recv_millis)) > (int)_params.allowed_latency); - + if((sys_tick.millis - _last_worst_latency_timestamp) > 5000) + { + _last_worst_latency_timestamp = sys_tick.millis; + _worst_latency_so_far = -1; + } + + if( (sys_tick.millis - db_input.DB_prev_MCU_recv_millis) > _worst_latency_so_far) + { + + _worst_latency_so_far = (sys_tick.millis - db_input.DB_prev_MCU_recv_millis); + + } + bool timing_failure = (message_too_latent || no_messages_received || drivebrain_has_not_received_time); diff --git a/platformio.ini b/platformio.ini index 4d816a994..5ef95794c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -4,7 +4,7 @@ lib_deps_shared = git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 https://github.com/hytech-racing/shared_firmware_types.git#feb3b83 https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - https://github.com/hytech-racing/HT_proto/releases/download/2024-10-04T04_27_55/hytech_msgs_pb_lib.tar.gz + https://github.com/hytech-racing/HT_proto/releases/download/2024-10-20T18_54_37/hytech_msgs_pb_lib.tar.gz [env:test_env] platform = native diff --git a/src/main.cpp b/src/main.cpp index 53552b3f6..4e7090c60 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,8 +11,7 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" -#include "pb.h" -#include "PrintLogger.h" +// #include "NativeEthernet.h" // /* Interfaces */ #include "DrivebrainETHInterface.h" @@ -54,8 +53,6 @@ /* PARAMETER STRUCTS */ -using namespace qindesign::network; - const TelemetryInterfaceReadChannels telem_read_channels = { .accel1_channel = MCU15_ACCEL1_CHANNEL, @@ -347,7 +344,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); -void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg); +void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg); /* SETUP @@ -434,19 +431,21 @@ void loop() // single source of truth for the state of the car. // no systems or interfaces should write directly to this. SharedCarState_s car_state_inst(curr_tick, - steering_system.getSteeringSystemData(), - drivetrain.get_dynamic_data(), - load_cell_interface.getLoadCellForces(), - pedals_system.getPedalsSystemData(), - vn_interface.get_vn_struct(), - db_eth_interface.get_latest_data(), - torque_controller_mux.get_tc_mux_status()); + steering_system.getSteeringSystemData(), + drivetrain.get_dynamic_data(), + load_cell_interface.getLoadCellForces(), + pedals_system.getPedalsSystemData(), + vn_interface.get_vn_struct(), + db_eth_interface.get_latest_data(), + torque_controller_mux.get_tc_mux_status()); hytech_msgs_MCUOutputData out_eth_msg = db_eth_interface.make_db_msg(car_state_inst); handle_ethernet_interface_comms(curr_tick, out_eth_msg); 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()) @@ -506,9 +505,6 @@ void loop() Serial.print("Current TC error: "); Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().active_error)); Serial.println(); - Serial.print("dial state: "); - Serial.println(static_cast(dashboard.getDialMode())); - Serial.println(); Serial.println(); } } @@ -591,8 +587,6 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) a1.get().conversions[MCU15_BRAKE2_CHANNEL], pedals_system.getMechBrakeActiveThreshold(), torque_controller_mux.get_tc_mux_status().active_error); - - ams_interface.tick(current_system_tick); } if (t.trigger50) // 50Hz @@ -614,6 +608,11 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) .RLConversion = sab_interface.rlLoadCell.convert(), .RRConversion = sab_interface.rrLoadCell.convert()}); } + load_cell_interface.update_raw_data((LoadCellInterfaceTick_s){ + .FLConversion = a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], + .FRConversion = a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], + .RLConversion = sab_interface.rlLoadCell.convert(), + .RRConversion = sab_interface.rrLoadCell.convert()}); // // Untriggered main_ecu.read_mcu_status(); // should be executed at the same rate as state machine // DO NOT call in main_ecu.tick() @@ -655,19 +654,21 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) fsm.get_state(), dashboard.startButtonPressed(), vn_interface.get_vn_struct().vn_status); - } -void handle_ethernet_interface_comms(const SysTick_s& systick, const hytech_msgs_MCUOutputData& out_msg) +void handle_ethernet_interface_comms(const SysTick_s &systick, const hytech_msgs_MCUOutputData &out_msg) { // 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); std::function recv_boi = &recv_pb_stream_msg; handle_ethernet_socket_receive<1024, hytech_msgs_MCUCommandData>(systick, &protobuf_recv_socket, recv_boi, db_eth_interface, hytech_msgs_MCUCommandData_fields); - if(systick.triggers.trigger500) + if (systick.triggers.trigger1000) { handle_ethernet_socket_send_pb(EthParams::default_TCU_ip, EthParams::default_protobuf_send_port, &protobuf_send_socket, out_msg, hytech_msgs_MCUOutputData_fields); } -} +} \ No newline at end of file