From cead7ab5554e8fa168d1d15a943a0411fb8a4fa5 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Sun, 9 Jun 2024 21:45:06 -0400 Subject: [PATCH] Feature/thermistor interface (#97) * added in state of charge estimation * added in tcmux status telemetry * added thermistor interface * added in checks for vn data into CASE for ensuring that if we have a bad velocity estimate we are still ok * made steering system only use bottom analog sensor * updated CASE parameters * removed steering system tests * switched from normal load gain scheduling to RPM based gain scheduling for TCS and set end bound for SL for fronts to 0.25 and decreased upper p bounds for TCS for front and rear --------- Co-authored-by: Eric Galluzzi Co-authored-by: CL16gtgh Co-authored-by: shaynoorani <113149316+shaynoorani@users.noreply.github.com> --- include/MCU_rev15_defs.h | 57 ++--- lib/interfaces/include/AMSInterface.h | 206 ++++++++++++++++-- lib/interfaces/include/HytechCANInterface.h | 8 +- lib/interfaces/include/TelemetryInterface.h | 51 +++-- lib/interfaces/include/ThermistorInterface.h | 44 ++++ lib/interfaces/src/AMSInterface.cpp | 161 ++++++++++++-- lib/interfaces/src/TelemetryInterface.cpp | 61 ++++-- lib/interfaces/src/ThermistorInterface.cpp | 40 ++++ lib/mock_interfaces/TelemetryInterface.h | 31 +++ lib/mock_interfaces/TorqueControllerData.h | 31 +++ .../include/TorqueControllersData.h | 23 +- lib/systems/include/CASESystem.tpp | 45 ++-- lib/systems/include/SteeringSystem.h | 25 ++- lib/systems/include/TorqueControllerMux.h | 55 +++-- lib/systems/include/TorqueControllers.h | 3 +- lib/systems/src/SteeringSystem.cpp | 91 +++----- lib/systems/src/TorqueControllerMux.cpp | 93 +++++++- lib/systems/src/TorqueControllers.cpp | 18 +- platformio.ini | 7 +- src/main.cpp | 57 +++-- test/test_systems/main.cpp | 3 +- test/test_systems/state_machine_test.h | 21 +- test/test_systems/steering_system_test.h | 21 +- .../test_systems/torque_controller_mux_test.h | 65 +++++- 24 files changed, 947 insertions(+), 270 deletions(-) create mode 100644 lib/interfaces/include/ThermistorInterface.h create mode 100644 lib/interfaces/src/ThermistorInterface.cpp create mode 100644 lib/mock_interfaces/TelemetryInterface.h create mode 100644 lib/mock_interfaces/TorqueControllerData.h diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 1ca6f11fb..249e6c891 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -8,9 +8,9 @@ // pindefs -const int ADC1_CS = 34; -const int ADC2_CS = 33; -const int ADC3_CS = 29; +const int ADC1_CS = 34; // Main ECU +const int ADC2_CS = 33; // FL corner board +const int ADC3_CS = 29; // FR corner board const int SOFTWARE_OK = 8; const int WATCHDOG_INPUT = 32; @@ -32,10 +32,11 @@ const int MCU15_FR_LOADCELL_CHANNEL = 2; //MCU teensy analog channels const int MCU15_TEENSY_ADC_CHANNELS = 2; +// const int MCU15_NUM_THERMISTORS = 2; const int MCU15_THERM_FL = 38; const int MCU15_THERM_FR = 41; -const int MCU15_THERM_FL_CHANNEL = 0; -const int MCU15_THERM_FR_CHANNEL = 1; +// const int MCU15_THERM_FL_CHANNEL = 0; +// const int MCU15_THERM_FR_CHANNEL = 1; const int DEFAULT_ANALOG_PINS[MCU15_TEENSY_ADC_CHANNELS] = {MCU15_THERM_FL, MCU15_THERM_FR}; // Time intervals @@ -43,6 +44,8 @@ const unsigned long SETUP_PRESENT_ACTION_INTERVAL = 1000; const unsigned long BUZZER_ON_INTERVAL = 2000; const unsigned long INVERTER_ENABLING_TIMEOUT_INTERVAL = 5000; +//glv sense conversion +const float GLV_SENSE_SCALE = .0073 * 4.0865; // Communication speeds const unsigned long INV_CAN_BAUDRATE = 500000; const unsigned long TELEM_CAN_BAUDRATE = 500000; @@ -51,17 +54,17 @@ const unsigned long TELEM_CAN_BAUDRATE = 500000; // All of these values are the PEDAL min/max // the sensor min/max that trip implaus are calculated // in the PedalsSystem constructor -const int ACCEL1_PEDAL_MAX = 3330; -const int ACCEL2_PEDAL_MAX = 388; +const int ACCEL1_PEDAL_MAX = 3409; // 3330 +const int ACCEL2_PEDAL_MAX = 259; // 388 -const int ACCEL1_PEDAL_MIN = 2124; +const int ACCEL1_PEDAL_MIN = 2129; const int ACCEL2_PEDAL_MIN = 1517; -const int BRAKE1_PEDAL_MAX = 2200; -const int BRAKE2_PEDAL_MAX = 2200; +const int BRAKE1_PEDAL_MAX = 1828; // 2200; +const int BRAKE2_PEDAL_MAX = 1857; // 2200; -const int BRAKE1_PEDAL_MIN = 785; -const int BRAKE2_PEDAL_MIN = 785; +const int BRAKE1_PEDAL_MIN = 1184; // 785; // 1230 to 1750 +const int BRAKE2_PEDAL_MIN = 2486; // 785; // 2450 to 1930 const int ACCEL1_PEDAL_OOR_MIN = 90; const int ACCEL2_PEDAL_OOR_MIN = 90; @@ -76,7 +79,7 @@ const int BRAKE1_PEDAL_OOR_MAX = 4000; const int BRAKE2_PEDAL_OOR_MAX = 4000; const float DEFAULT_PEDAL_DEADZONE = 0.05f; -const float DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN = 0.10f; +const float DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN = 0.12f; const float APPS_ACTIVATION_PERCENTAGE = 0.1f; const float BRAKE_ACTIVATION_PERCENTAGE = 0.05f; @@ -85,24 +88,28 @@ const float BRAKE_MECH_THRESH = 0.40f; // Load Cell Defs to convert raw to lbs // lbs = (raw + offset) * scale -const float LOADCELL_FL_SCALE = 0.0554; -const float LOADCELL_FL_OFFSET = 19.976 / LOADCELL_FL_SCALE; +const float LOADCELL_FL_SCALE = 0.1053; +const float LOADCELL_FL_OFFSET = 19.674 / LOADCELL_FL_SCALE; -const float LOADCELL_FR_SCALE = 0.0514; -const float LOADCELL_FR_OFFSET = 19.892 / LOADCELL_FR_SCALE; +const float LOADCELL_FR_SCALE = 0.097; +const float LOADCELL_FR_OFFSET = 21.183 / LOADCELL_FR_SCALE; -const float LOADCELL_RL_SCALE = 0.0595; -const float LOADCELL_RL_OFFSET = 7.148 / LOADCELL_RL_SCALE; +const float LOADCELL_RL_SCALE = 0.1149; +const float LOADCELL_RL_OFFSET = 13.526 / LOADCELL_RL_SCALE; -const float LOADCELL_RR_SCALE = 0.06; -const float LOADCELL_RR_OFFSET = 23.761 / LOADCELL_RR_SCALE; +const float LOADCELL_RR_SCALE = 0.118; +const float LOADCELL_RR_OFFSET = 25.721 / LOADCELL_RR_SCALE; // Steering parameters const float PRIMARY_STEERING_SENSE_OFFSET = 0.0; // units are degrees -const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 785; // 794 // 812 // 130 deg -const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3087; // 3075 // 3179 // 134 deg -const int SECONDARY_STEERING_SENSE_CENTER = 1945; // 1960 // 1970 -const float STEERING_RANGE_DEGREES = 257.0f; // 253.0f // 256.05f // 134+130-7(slop) +const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 790; // 785; // 794 // 812 // 130 deg // 128.95 +const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3133; // 3087; // 3075 // 3179 // 134 deg // 132.79 +const int SECONDARY_STEERING_SENSE_CENTER = 1985; // 1945 // 1960 // 1970 +const float STEERING_RANGE_DEGREES = 254.74; //257.0f // 253.0f // 256.05f // 134+130-7(slop) const float STEERING_IIR_ALPHA = 0.7f; // shaves off around 1 deg of max discrepancy +// TC parameters +const float SIMPLE_TC_REAR_TORQUE_SCALE = 1.0; +const float SIMPLE_TC_REGEN_TORQUE_SCALE = 0.4; + #endif /* __MCU15_H__ */ \ No newline at end of file diff --git a/lib/interfaces/include/AMSInterface.h b/lib/interfaces/include/AMSInterface.h index 4227c3a37..38dde714a 100644 --- a/lib/interfaces/include/AMSInterface.h +++ b/lib/interfaces/include/AMSInterface.h @@ -3,6 +3,9 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" +#include "SysClock.h" +#include "hytech.h" +#include "MessageQueueDefine.h" /* Heartbeat Interval is the allowable amount of time between BMS status messages before car delatches */ const unsigned long HEARTBEAT_INTERVAL = 2000; // milliseconds @@ -15,6 +18,8 @@ const float DEFAULT_INIT_TEMP = 40.0; const float DEFAULT_INIT_VOLTAGE = 3.5; const float DEFAULT_TEMP_ALPHA = 0.8; const float DEFAULT_VOLTAGE_ALPHA = 0.8; +const uint16_t MAX_PACK_CHARGE = 48600; +const unsigned long DEFAULT_INITIALIZATION_WAIT_INTERVAL = 5000; /// @brief this class is for interfacing with the AMS (accumulator management system) @@ -26,19 +31,26 @@ class AMSInterface @param sw_ok_pin The software ok pin number. This pin is connected to the shutdown line and will go low if the AMS times out */ - AMSInterface(int sw_ok_pin, float init_temp, float init_volt, float temp_alpha, float volt_alpha): + AMSInterface(CANBufferType *msg_output_queue, int sw_ok_pin, float init_temp, float init_volt, float temp_alpha, float volt_alpha): + msg_queue_(msg_output_queue), pin_software_ok_(sw_ok_pin), filtered_max_cell_temp(init_temp), filtered_min_cell_voltage(init_volt), cell_temp_alpha(temp_alpha), - cell_voltage_alpha(volt_alpha) {}; + cell_voltage_alpha(volt_alpha), + use_em_for_soc_(true), + charge_(0.0f), + SoC_(0.0f), + has_initialized_charge_(false), + has_received_bms_voltage_(false), + initialization_startup_interval_(DEFAULT_INITIALIZATION_WAIT_INTERVAL) {}; /* Overloaded constructor that only takes in software OK pin and uses default voltages and temp*/ - AMSInterface(int sw_ok_pin): - AMSInterface(sw_ok_pin, DEFAULT_INIT_TEMP, DEFAULT_INIT_VOLTAGE, DEFAULT_TEMP_ALPHA, DEFAULT_VOLTAGE_ALPHA) {}; + AMSInterface(CANBufferType *msg_output_queue, int sw_ok_pin): + AMSInterface(msg_output_queue, sw_ok_pin, DEFAULT_INIT_TEMP, DEFAULT_INIT_VOLTAGE, DEFAULT_TEMP_ALPHA, DEFAULT_VOLTAGE_ALPHA) {}; /* Initialize the heartbeat timer */ - void init(unsigned long curr_millis); + void init(SysTick_s &initial_tick); /* Init software OK pin by setting high*/ void set_start_state(); @@ -60,32 +72,143 @@ class AMSInterface float get_filtered_max_cell_temp(); /* IIR filter and return filtered min cell voltage */ float get_filtered_min_cell_voltage(); + /*gets the derate factor for acc system*/ + float get_acc_derate_factor(); + + /** + * Initializes the charge member variable from the voltage of the minimum cell using the VOLTAGE_LOOKUP_TABLE. + * + * @pre The bms_voltages_ member variable MUST be initialized before this function can be called. + * @return The charge, in coulombs, that the charge member variable is initialized to. + */ + float initialize_charge(); + + /** + * Calculates SoC based on the energy meter CAN message. Calling calculate_SoC_em() + * will update the SoC_ and charge_ member variables. + * + * @param The current tick that the calculate_SoC_em function should use for integration. + * + * @pre The last_tick_ member variable must be updated correctly. + * @post The charge_ field has the updated charge, and the SoC field contains an updated percentage. + */ + void calculate_SoC_em(const SysTick_s &tick); + + /** + * Calculates SoC based on the ACU_SHUNT_MEASUREMENTS CAN message. Calling calculate_SoC_acu() + * will update the SoC_ and charge_ member variables. + * + * @param The current tick that the calculate_SoC_em function should use for integration. + * + * @pre The last_tick_ member variable must be updated correctly. + * @post The charge_ field has the updated charge, and the SoC field contains an updated percentage. + */ + void calculate_SoC_acu(const SysTick_s &tick); + + /** + * Retrieves the value of the SoC member variable. This function does NOT recalculate + * the SoC_ variable, it only returns the value that is stored. + * + * @return the current value stored in the SoC_ member variable. + */ + float get_SoC() {return SoC_;} + + /** + * This is AMSInterface's tick() function. It behaves correctly regardless of the + * since the functions calculate the elapsed time between the given tick and the stored last_tick_. + * + * @param tick The current system tick. + */ + void tick(const SysTick_s &tick); //RETRIEVE CAN MESSAGES// - /* read BMS status messages */ + + /** + * Reads this CAN message into the bms_status_ member variable. This function uses the OLD CAN library. + */ void retrieve_status_CAN(unsigned long curr_millis, CAN_message_t &recvd_msg); - /* read BMS temperature messages */ + + /** + * Reads this CAN message into the bms_temperatures_ member variable. This function uses the OLD CAN library. + */ void retrieve_temp_CAN(CAN_message_t &recvd_msg); - /* read BMS voltage messages */ + + /** + * Reads this CAN message into the bms_voltages_ member variable. This function + * does NOT apply the fromS() functions on the data. This function uses the NEW CAN library. + */ void retrieve_voltage_CAN(CAN_message_t &recvd_msg); + + /*Updates Acc_derate_factor*/ + void calculate_acc_derate_factor(); + + /** + * Reads this CAN message into the em_measurements_ member variable. This function + * does NOT apply the fromS() functions on the data. This function uses the NEW CAN library. + */ + void retrieve_em_measurement_CAN(CAN_message_t &can_msg); + + /** + * Reads this CAN message into the acu_shunt_measurements_ member variable. This function + * does NOT apply the fromS() functions on the data. This function uses the NEW CAN library. + */ + void retrieve_current_shunt_CAN(const CAN_message_t &can_msg); + + /** + * Retrieves the current state of the use_em_for_soc member variable. + * @return True if using EM, false if using ACU shunt measurements. + */ + bool is_using_em_for_soc() {return use_em_for_soc_;} + + /** + * Setter function for the use_em_for_soc_ member variable. + * @param new_use_em_for_soc The new value for the variable. + */ + void set_use_em_for_soc(bool new_use_em_for_soc) { + use_em_for_soc_ = new_use_em_for_soc; + } + + /** + * Puts the current value of SoC_ and charge_ onto msg_queue_. + */ + void enqueue_state_of_charge_CAN(); + + /** Helper function to enqueue CAN messages using the new CAN library*/ + template + void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *)); + + // Getters (for testing purposes) + BMS_VOLTAGES_t get_bms_voltages() {return bms_voltages_;} + EM_MEASUREMENT_t get_em_measurements() {return em_measurements_;} + ACU_SHUNT_MEASUREMENTS_t get_acu_shunt_measurements() {return acu_shunt_measurements_;} + private: - /* Private functions */ - // Check if lowest cell temperature is below threshold - bool is_below_pack_charge_critical_low_thresh(); - // Check if total pack charge is above threshold - bool is_below_pack_charge_critical_total_thresh(); + + const float VOLTAGE_LOOKUP_TABLE[101] = {3.972, 3.945, 3.918, 3.891, 3.885, 3.874, 3.864, 3.858, 3.847, 3.836, 3.82, 3.815, 3.815, 3.798, 3.788, + 3.782, 3.771, 3.755, 3.744, 3.744, 3.733, 3.728, 3.723, 3.712, 3.701, 3.695, 3.69, 3.679, 3.679, 3.668, 3.663, 3.657, 3.647, + 3.647, 3.636, 3.625, 3.625, 3.625, 3.614, 3.609, 3.603, 3.603, 3.592, 3.592, 3.592, 3.581, 3.581, 3.571, 3.571, 3.571, 3.56, + 3.56, 3.56, 3.549, 3.549, 3.549, 3.549, 3.538, 3.538, 3.551, 3.546, 3.535, 3.535, 3.535, 3.53, 3.524, 3.524, 3.524, 3.513, + 3.513, 3.513, 3.503, 3.503, 3.492, 3.492, 3.492, 3.487, 3.481, 3.481, 3.476, 3.471, 3.46, 3.46, 3.449, 3.444, 3.428, 3.428, + 3.417, 3.401, 3.39, 3.379, 3.363, 3.331, 3.299, 3.267, 3.213, 3.149, 3.041, 3, 3, 0}; + + /** + * CAN line that this AMSInterface should write to. This should be the Telemetry CAN line. + */ + CANBufferType *msg_queue_; + + /* software OK pin */ + int pin_software_ok_; /* AMS CAN messages */ BMS_status bms_status_; BMS_temperatures bms_temperatures_; - BMS_voltages bms_voltages_; + ACU_SHUNT_MEASUREMENTS_t acu_shunt_measurements_; + EM_MEASUREMENT_t em_measurements_; + BMS_VOLTAGES_t bms_voltages_; /* AMS last heartbeat time */ - unsigned long last_heartbeat_time; - - /* software OK pin */ - int pin_software_ok_; + unsigned long last_heartbeat_time_; /* IIR filter parameters */ float bms_high_temp; @@ -94,6 +217,51 @@ class AMSInterface float filtered_min_cell_voltage; float cell_temp_alpha; float cell_voltage_alpha; + + float acc_derate_factor; + + /** + * If set to TRUE, then SoC will use EM. + * If set to FALSE, then SoC will use ACU SHUNT. + * Set to TRUE in constructor by default. + */ + bool use_em_for_soc_ = true; + + /** + * The charge stored on the accumulator. Stored in coulombs, ranging from + * zero to MAX_PACK_CHARGE. + */ + float charge_; + + /** + * Stores the current state of charge of the accumulator. SoC is stored as a + * percentage of MAX_PACK_CHARGE. In every location, this is calculated as + * SoC = (charge / MAX_PACK_CHARGE) * 100; + */ + float SoC_; + + /** + * Stores the last Sys_Tick_s struct from the last time the tick() function is called. + */ + SysTick_s last_tick_; + + /** + * Stores whether or not this AMSInterface has initialized SoC_ or not. + */ + bool has_initialized_charge_; + + bool has_received_bms_voltage_; + + unsigned long initialization_startup_interval_; + + unsigned long timestamp_start_; + + + // Check if lowest cell temperature is below threshold + bool is_below_pack_charge_critical_low_thresh(); + // Check if total pack charge is above threshold + bool is_below_pack_charge_critical_total_thresh(); + }; -#endif /* __AMSINTERFACE_H__ */ +#endif /* __AMSINTERFACE_H__ */ \ No newline at end of file diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 92bdd33f5..7e1c47235 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -99,6 +99,12 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case ID_BMS_VOLTAGES: interfaces.ams_interface->retrieve_voltage_CAN(recvd_msg); break; + case ID_EM_MEASUREMENT: + interfaces.ams_interface->retrieve_em_measurement_CAN(recvd_msg); + break; + case ACU_SHUNT_MEASUREMENTS_CANID: + interfaces.ams_interface->retrieve_current_shunt_CAN(recvd_msg); + break; // MC status msgs case ID_MC1_STATUS: @@ -219,4 +225,4 @@ void enqueue_matlab_msg(bufferType *msg_queue, const CAN_MESSAGE_BUS & structure } -#endif /* HYTECHCANINTERFACE */ +#endif /* HYTECHCANINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index c94223e3e..c9b360eb9 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -13,7 +13,7 @@ #include "TorqueControllersData.h" using InvInt_t = InverterInterface; -const int FIXED_POINT_PRECISION = 10000; +const int FIXED_POINT_PRECISION = 1000; struct TelemetryInterfaceReadChannels { @@ -47,7 +47,7 @@ class TelemetryInterface TelemetryInterface(CANBufferType *msg_output_queue, const TelemetryInterfaceReadChannels &channels) : msg_queue_(msg_output_queue), channels_(channels){}; - /* GETTERS for basic conversions that don't have a home*/ + /* GETTERS for basic conversions that don't have a home */ AnalogConversion_s get_glv_voltage(const AnalogConversionPacket_s<8> &adc1) { @@ -56,10 +56,6 @@ class TelemetryInterface /* Update CAN messages (main loop) */ // Interfaces - void update_front_thermistors_CAN_msg( - const AnalogConversion_s &therm_fl, - const AnalogConversion_s &therm_fr - ); void update_pedal_readings_CAN_msg( float accel_percent, float brake_percent, @@ -75,23 +71,27 @@ class TelemetryInterface const AnalogConversion_s &lc_fl, const AnalogConversion_s &lc_fr, const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr); + const AnalogConversion_s &pots_fr + ); void update_analog_readings_CAN_msg( const SteeringEncoderConversion_s &steer1, const AnalogConversion_s &steer2, const AnalogConversion_s ¤t, const AnalogConversion_s &reference, - const AnalogConversion_s &glv); + const AnalogConversion_s &glv + ); void update_drivetrain_rpms_CAN_msg( InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); void update_drivetrain_err_status_CAN_msg( InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); void update_drivetrain_status_telem_CAN_msg( InvInt_t *fl, InvInt_t *fr, @@ -100,38 +100,45 @@ class TelemetryInterface bool accel_implaus, bool brake_implaus, float accel_per, - float brake_per); + float brake_per + ); void update_drivetrain_torque_telem_CAN_msg( InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); void update_penthouse_accum_CAN_msg( const AnalogConversion_s ¤t, - const AnalogConversion_s &reference); - - void enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data); + const AnalogConversion_s &reference + ); + void update_TCMux_status_CAN_msg( + const TCMuxStatus_s &tcMuxStatus + ); + void update_steering_status_CAN_msg( + const float steering_system_angle, + const float filtered_angle_encoder, + const float filtered_angle_analog, + const uint8_t steering_system_status, + const uint8_t steering_encoder_status, + const uint8_t steering_analog_status + ); /* Enqueue outbound telemetry CAN messages */ - // void enqueue_CAN_mcu_pedal_readings(); - // void enqueue_CAN_mcu_load_cells(); - // void enqueue_CAN_mcu_front_potentiometers(); - // void enqueue_CAN_mcu_rear_potentiometers(); - // void enqueue_CAN_mcu_analog_readings(); - template void enqueue_CAN(T can_msg, uint32_t id); template void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *)); + void enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data); + // TODO dont pass in pointer to inverter interface here we will break shit with this /* Tick at 50Hz to send CAN */ void tick( const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, - const AnalogConversionPacket_s<2> &mcu_adc, const SteeringEncoderConversion_s &encoder, InvInt_t *fl, InvInt_t *fr, diff --git a/lib/interfaces/include/ThermistorInterface.h b/lib/interfaces/include/ThermistorInterface.h new file mode 100644 index 000000000..4c49f3ec6 --- /dev/null +++ b/lib/interfaces/include/ThermistorInterface.h @@ -0,0 +1,44 @@ +#ifndef THERMISTORINTERFACE +#define THERMISTORINTERFACE +#include "stdint.h" +#include +#include "FlexCAN_T4.h" +#include "HyTech_CAN.h" +#include "MessageQueueDefine.h" +#include "AnalogSensorsInterface.h" +#include "hytech.h" +#include "Thermistor.h" + +const int MCU15_NUM_THERMISTORS = 2; +const int MCU15_THERM_FL_CHANNEL = 0; +const int MCU15_THERM_FR_CHANNEL = 1; + +class ThermistorInterface +{ +private: + Thermistors front_thermistors; + + float therm_fl; + float therm_fr; + + CANBufferType *_msg_queue; + + float convert(int raw); + +public: + ThermistorInterface(CANBufferType *msg_output_queue); + void update_front_thermistor_readings(); + + template + void enqueue_CAN_front_thermistors(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *)); + + void tick(const AnalogConversion_s &raw_therm_fl, const AnalogConversion_s &raw_therm_fr); + +}; + + + + + + +#endif \ No newline at end of file diff --git a/lib/interfaces/src/AMSInterface.cpp b/lib/interfaces/src/AMSInterface.cpp index 14af00e5a..6a930c663 100644 --- a/lib/interfaces/src/AMSInterface.cpp +++ b/lib/interfaces/src/AMSInterface.cpp @@ -1,11 +1,31 @@ #include "AMSInterface.h" +#include "SysClock.h" + +/* Send inverter CAN messages with new CAN library */ +template +void AMSInterface::enqueue_new_CAN(U* structure, uint32_t (* pack_function)(U*, uint8_t*, uint8_t*, uint8_t*)) { + CAN_message_t can_msg; + can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + uint8_t buf[sizeof(CAN_message_t)] = {}; + memmove(buf, &can_msg, sizeof(CAN_message_t)); + msg_queue_->push_back(buf, sizeof(CAN_message_t)); +} -void AMSInterface::init(unsigned long curr_millis) { +void AMSInterface::init(SysTick_s &initial_tick) { // Set pin mode pinMode(pin_software_ok_, OUTPUT); - set_heartbeat(curr_millis); + set_heartbeat(initial_tick.millis); + + last_tick_ = initial_tick; + + timestamp_start_ = last_tick_.millis; + + // Initializes the bms_voltages_ member variable to an invalid state. This will + // get overridden once retrieve_voltage_CAN() has been called at least once. + bms_voltages_.low_voltage_ro = 0xFFFFU; + bms_voltages_.high_voltage_ro = 0x1111U; } @@ -24,26 +44,19 @@ void AMSInterface::set_state_ok_high(bool ok_high) { } void AMSInterface::set_heartbeat(unsigned long curr_millis) { - last_heartbeat_time = curr_millis; + last_heartbeat_time_ = curr_millis; } bool AMSInterface::heartbeat_received(unsigned long curr_millis) { - // if((curr_millis - last_heartbeat_time) >= HEARTBEAT_INTERVAL){ - // Serial.println("ERROR"); - // Serial.println(curr_millis); - // Serial.println(last_heartbeat_time); - // Serial.println(curr_millis - last_heartbeat_time); - // } - - return ((curr_millis - last_heartbeat_time) < HEARTBEAT_INTERVAL); + return ((curr_millis - last_heartbeat_time_) < HEARTBEAT_INTERVAL); } bool AMSInterface::is_below_pack_charge_critical_low_thresh() { - return (bms_voltages_.get_low() < PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD); + return (HYTECH_low_voltage_ro_fromS(bms_voltages_.low_voltage_ro) < PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD); } bool AMSInterface::is_below_pack_charge_critical_total_thresh() { - return (bms_voltages_.get_total() < PACK_CHARGE_CRIT_TOTAL_THRESHOLD); + return (HYTECH_total_voltage_ro_fromS(bms_voltages_.total_voltage_ro) < PACK_CHARGE_CRIT_TOTAL_THRESHOLD); } bool AMSInterface::pack_charge_is_critical() { @@ -58,11 +71,85 @@ float AMSInterface::get_filtered_max_cell_temp() { } float AMSInterface::get_filtered_min_cell_voltage() { - bms_low_voltage = bms_voltages_.get_low() / 10000.0; + bms_low_voltage = HYTECH_low_voltage_ro_fromS(bms_voltages_.low_voltage_ro); filtered_min_cell_voltage = filtered_min_cell_voltage * cell_temp_alpha + (1.0 - cell_voltage_alpha) * bms_low_voltage; return filtered_min_cell_voltage; } +float AMSInterface::initialize_charge() { + int i = 0; + while (HYTECH_low_voltage_ro_fromS(bms_voltages_.low_voltage_ro) - VOLTAGE_LOOKUP_TABLE[i] < 0) { + i++; + } + charge_ = ( (100 - i) / 100.0) * MAX_PACK_CHARGE; + SoC_ = (charge_ / MAX_PACK_CHARGE) * 100; + + return charge_; +} + +void AMSInterface::calculate_SoC_em(const SysTick_s &tick) { + unsigned long delta_time_micros = tick.micros - last_tick_.micros; + + float current = HYTECH_em_current_ro_fromS(em_measurements_.em_current_ro); // Current in amps + + // coulombs = amps * microseconds * (1sec / 1000000 microsec) + charge_ -= (current * delta_time_micros) / 1000000; + + SoC_ = (charge_ / MAX_PACK_CHARGE) * 100; +} + +void AMSInterface::calculate_SoC_acu(const SysTick_s &tick) { + unsigned long delta_time_micros = tick.micros - last_tick_.micros; + + // Converts analog read (from 0 to 4095) into some value (0.0 to 3.3) + // float current = HYTECH_current_shunt_read_ro_fromS(acu_shunt_measurements_.current_shunt_read_ro); + + // shunt_voltage ranges from -3.33 to 2.635 + // float shunt_voltage = (current * (9.22 / 5.1)) - 3.3 - 0.03; + + // calc_current ranges from -666 to 527.176 + // float calc_current = (shunt_voltage / 0.005); + charge_ -= (HYTECH_current_shunt_read_ro_fromS(acu_shunt_measurements_.current_shunt_read_ro) * delta_time_micros) / 1000000; + + SoC_ = (charge_ / MAX_PACK_CHARGE) * 100; +} + +void AMSInterface::tick(const SysTick_s &tick) { + + // If AMSInterface has a valid reading in bms_voltages_ and the charge is not + // yet initialized, then call initialize_charge. + if ((!has_initialized_charge_) && (has_received_bms_voltage_) && ((tick.millis - timestamp_start_) > initialization_startup_interval_)) { + + bool bms_voltages_is_invalid = bms_voltages_.low_voltage_ro == 0xFFFFU && bms_voltages_.high_voltage_ro == 0x1111U; + + if (!bms_voltages_is_invalid) { + initialize_charge(); + has_initialized_charge_ = true; + } + + } + + // Only calculate the updated SoC if charge has been properly initialized. + if (has_initialized_charge_) { + // Do not edit this block! If both calculate_SoC_em AND calculate_SoC_acu are run, + // then the charge will be subtracted from the SoC member variable twice. + if (use_em_for_soc_) { + calculate_SoC_em(tick); + } else { + calculate_SoC_acu(tick); + } + } + + // Send CAN message + // enqueue_state_of_charge_CAN(); + STATE_OF_CHARGE_t soc_struct; + soc_struct.charge_percentage_ro = HYTECH_charge_percentage_ro_toS(SoC_); + soc_struct.charge_coulombs_ro = HYTECH_charge_coulombs_ro_toS(charge_); + enqueue_new_CAN(&soc_struct, Pack_STATE_OF_CHARGE_hytech); + + last_tick_ = tick; +} + //RETRIEVE CAN MESSAGES// void AMSInterface::retrieve_status_CAN(unsigned long curr_millis, CAN_message_t &recvd_msg) { bms_status_.load(recvd_msg.buf); @@ -73,8 +160,50 @@ void AMSInterface::retrieve_temp_CAN(CAN_message_t &recvd_msg) { bms_temperatures_.load(recvd_msg.buf); } -void AMSInterface::retrieve_voltage_CAN(CAN_message_t &recvd_msg) { - bms_voltages_.load(recvd_msg.buf); +void AMSInterface::retrieve_voltage_CAN(CAN_message_t &can_msg) { + Unpack_BMS_VOLTAGES_hytech(&bms_voltages_, can_msg.buf, can_msg.len); + if (!has_received_bms_voltage_) + { + has_received_bms_voltage_ = true; + } + +} + +void AMSInterface::retrieve_em_measurement_CAN(CAN_message_t &can_msg) { + Unpack_EM_MEASUREMENT_hytech(&em_measurements_, can_msg.buf, can_msg.len); +} + +void AMSInterface::retrieve_current_shunt_CAN(const CAN_message_t &can_msg) { + Unpack_ACU_SHUNT_MEASUREMENTS_hytech(&acu_shunt_measurements_, can_msg.buf, can_msg.len); +} +void AMSInterface::calculate_acc_derate_factor() { + float voltage_lim_factor = 1.0; + float startDerateVoltage = 3.5; + float endDerateVoltage = 3.2; + float voltage_lim_max = 1; + float voltage_lim_min = 0.2; + + float temp_lim_factor = 1.0; + float startDerateTemp = 50; + float stopDerateTemp = 58; + float temp_lim_max = 1; + float temp_lim_min = 0.2; + + float filtered_min_cell_voltage = get_filtered_min_cell_voltage(); + //float_map equivalient because new code is bad + voltage_lim_factor = (filtered_min_cell_voltage - startDerateVoltage) * (voltage_lim_min - voltage_lim_max) / (endDerateVoltage - startDerateVoltage) + voltage_lim_max; + voltage_lim_factor = max(min(voltage_lim_max, voltage_lim_factor), voltage_lim_min); + + temp_lim_factor = (filtered_max_cell_temp - startDerateTemp) * (temp_lim_min - temp_lim_max) / (stopDerateTemp - startDerateTemp) + temp_lim_max; + temp_lim_factor = max(min(temp_lim_factor, temp_lim_max), temp_lim_min); + + acc_derate_factor = min(temp_lim_factor,voltage_lim_factor); +} + +float AMSInterface::get_acc_derate_factor() { + calculate_acc_derate_factor(); + return acc_derate_factor; + } diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 963a83740..b6c9ff0fb 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -36,8 +36,8 @@ void TelemetryInterface::update_suspension_CAN_msg(const AnalogConversion_s &lc_ MCU_SUSPENSION_t sus; sus.load_cell_fl = lc_fl.raw; sus.load_cell_fr = lc_fr.raw; - sus.potentiometer_fl = pots_fr.raw; - sus.potentiometer_fr = pots_fl.raw; + sus.potentiometer_fl = pots_fl.raw; + sus.potentiometer_fr = pots_fr.raw; enqueue_new_CAN(&sus, &Pack_MCU_SUSPENSION_hytech); } @@ -50,22 +50,11 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon // do sth with mcu_analog_readings_ mcu_analog_readings_.set_steering_1(steer1.raw); mcu_analog_readings_.set_steering_2(steer2.raw); - mcu_analog_readings_.set_hall_effect_current(current.raw - reference.raw); - // Serial.println("hall effect current: "); - // Serial.println(mcu_analog_readings_.get_hall_effect_current()); - mcu_analog_readings_.set_glv_battery_voltage(glv.raw); + mcu_analog_readings_.set_hall_effect_current(current.raw - reference.raw); // this is wrong btw. should let analog channel do the math but not necessary atm + mcu_analog_readings_.set_glv_battery_voltage(glv.conversion * FIXED_POINT_PRECISION); enqueue_CAN(mcu_analog_readings_, ID_MCU_ANALOG_READINGS); } -void TelemetryInterface::update_front_thermistors_CAN_msg(const AnalogConversion_s &therm_fl, - const AnalogConversion_s &therm_fr) { - - FRONT_THERMISTORS_t front_thermistors_; - front_thermistors_.thermistor_motor_fl = therm_fl.raw; - front_thermistors_.thermistor_motor_fr = therm_fr.raw; - - enqueue_new_CAN(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); -} void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) @@ -194,6 +183,42 @@ void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s enqueue_new_CAN(&message, &Pack_PENTHOUSE_ACCUM_MSG_hytech); } +void TelemetryInterface::update_TCMux_status_CAN_msg(const TCMuxStatus_s &tcMuxStatus) +{ + TCMUX_STATUS_REPORT_t msg; + + msg.speed_above_thresh = tcMuxStatus.speedPreventsModeChange; + msg.torque_delta_above_thresh = tcMuxStatus.torqueDeltaPreventsModeChange; + msg.tc_not_ready = tcMuxStatus.controllerNotReadyPreventsModeChange; + msg.steering_system_has_err = tcMuxStatus.steeringSystemError; + msg.mode_intended = tcMuxStatus.modeIntended; + msg.mode_actual = tcMuxStatus.modeActual; + msg.dash_dial_mode = tcMuxStatus.dialMode; + msg.torque_mode = tcMuxStatus.torqueMode; + msg.torque_limit_ro = HYTECH_torque_limit_ro_toS(tcMuxStatus.maxTorque); + + enqueue_new_CAN(&msg, &Pack_TCMUX_STATUS_REPORT_hytech); +} + +void TelemetryInterface::update_steering_status_CAN_msg(const float steering_system_angle, + const float filtered_angle_encoder, + const float filtered_angle_analog, + const uint8_t steering_system_status, + const uint8_t steering_encoder_status, + const uint8_t steering_analog_status) +{ + STEERING_SYSTEM_REPORT_t msg; + + msg.steering_system_angle_ro = HYTECH_steering_system_angle_ro_toS(steering_system_angle); + msg.steering_encoder_angle_ro = HYTECH_steering_encoder_angle_ro_toS(filtered_angle_encoder); + msg.steering_analog_angle_ro = HYTECH_steering_analog_angle_ro_toS(filtered_angle_analog); + msg.steering_system_status = steering_system_status; + msg.steering_encoder_status = steering_encoder_status; + msg.steering_analog_status = steering_analog_status; + + enqueue_new_CAN(&msg, &Pack_STEERING_SYSTEM_REPORT_hytech); +} + /* Send CAN messages */ template void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { @@ -237,12 +262,10 @@ void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerDa } - /* Tick SysClock */ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, - const AnalogConversionPacket_s<2> &mcu_adc, const SteeringEncoderConversion_s &encoder, InvInt_t* fl, InvInt_t* fr, @@ -273,7 +296,7 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, adc1.conversions[channels_.analog_steering_channel], adc1.conversions[channels_.current_channel], adc1.conversions[channels_.current_ref_channel], - get_glv_voltage(adc1)); + adc1.conversions[channels_.glv_sense_channel]); // Load cells update_suspension_CAN_msg(adc2.conversions[channels_.loadcell_fl_channel], adc3.conversions[channels_.loadcell_fr_channel], @@ -289,7 +312,5 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, adc1.conversions[channels_.current_ref_channel]); enqeue_controller_CAN_msg(data); - update_front_thermistors_CAN_msg(mcu_adc.conversions[channels_.therm_fl_channel], - mcu_adc.conversions[channels_.therm_fr_channel]); } \ No newline at end of file diff --git a/lib/interfaces/src/ThermistorInterface.cpp b/lib/interfaces/src/ThermistorInterface.cpp new file mode 100644 index 000000000..76f8ce6fa --- /dev/null +++ b/lib/interfaces/src/ThermistorInterface.cpp @@ -0,0 +1,40 @@ +#include "ThermistorInterface.h" + + + + +ThermistorInterface::ThermistorInterface(CANBufferType *msg_output_queue) : front_thermistors() +{ + _msg_queue = msg_output_queue; + +} + + + +void ThermistorInterface::update_front_thermistor_readings() +{ + FRONT_THERMISTORS_t front_thermistors_; + //scale by 500 for easy packing + front_thermistors_.thermistor_motor_fl_ro = HYTECH_thermistor_motor_fl_ro_toS(therm_fl); + front_thermistors_.thermistor_motor_fr_ro = HYTECH_thermistor_motor_fr_ro_toS(therm_fr); + + enqueue_CAN_front_thermistors(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); +} + +template +void ThermistorInterface::enqueue_CAN_front_thermistors(U* structure, uint32_t (* pack_function)(U*, uint8_t*, uint8_t*, uint8_t*)) +{ + CAN_message_t can_msg; + can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + uint8_t buf[sizeof(CAN_message_t)] = {}; + memmove(buf, &can_msg, sizeof(CAN_message_t)); + _msg_queue->push_back(buf, sizeof(CAN_message_t)); +} + +void ThermistorInterface::tick(const AnalogConversion_s &raw_therm_fl, const AnalogConversion_s &raw_therm_fr) +{ + + therm_fl = front_thermistors.get(MCU15_THERM_FL_CHANNEL).convert(raw_therm_fl.raw); + therm_fr = front_thermistors.get(MCU15_THERM_FR_CHANNEL).convert(raw_therm_fr.raw); + update_front_thermistor_readings(); +} \ No newline at end of file diff --git a/lib/mock_interfaces/TelemetryInterface.h b/lib/mock_interfaces/TelemetryInterface.h new file mode 100644 index 000000000..ab1cf8a1c --- /dev/null +++ b/lib/mock_interfaces/TelemetryInterface.h @@ -0,0 +1,31 @@ +#ifndef __TELEMETRY_INTERFACE__ +#define __TELEMETRY_INTERFACE__ + +#include + +#include "TorqueControllerData.h" + +class TelemetryInterface +{ +private: + /* data */ +public: + TelemetryInterface(/* args */) {}; + ~TelemetryInterface() {}; + + void update_TCMux_status_CAN_msg( + const TCMuxStatus_s &tcMuxStatus + ) {}; + + void update_steering_status_CAN_msg( + const float steering_system_angle, + const float filtered_angle_encoder, + const float filtered_angle_analog, + const uint8_t steering_system_status, + const uint8_t steering_encoder_status, + const uint8_t steering_analog_status + ) {}; +}; + + +#endif \ No newline at end of file diff --git a/lib/mock_interfaces/TorqueControllerData.h b/lib/mock_interfaces/TorqueControllerData.h new file mode 100644 index 000000000..c35a0cb51 --- /dev/null +++ b/lib/mock_interfaces/TorqueControllerData.h @@ -0,0 +1,31 @@ +#ifndef __TORQUE_CONTROLLERS_DATA__ +#define __TORQUE_CONTROLLERS_DATA__ + +#include + +struct TCMuxStatus_s +{ + bool speedPreventsModeChange; + bool torqueDeltaPreventsModeChange; + bool controllerNotReadyPreventsModeChange; + bool steeringSystemError; + + uint8_t modeIntended; + uint8_t modeActual; + uint8_t dialMode; + + uint8_t torqueMode; + float maxTorque; +}; + +struct PIDTVTorqueControllerData +{ + float controller_input; + float controller_output; + float fl_torque_delta; + float fr_torque_delta; + float rl_torque_delta; + float rr_torque_delta; +}; + +#endif /* __TORQUE_CONTROLLERS_DATA__ */ \ No newline at end of file diff --git a/lib/shared_data/include/TorqueControllersData.h b/lib/shared_data/include/TorqueControllersData.h index f54d510a3..c35a0cb51 100644 --- a/lib/shared_data/include/TorqueControllersData.h +++ b/lib/shared_data/include/TorqueControllersData.h @@ -1,5 +1,22 @@ -#ifndef TORQUECONTROLLERSDATA -#define TORQUECONTROLLERSDATA +#ifndef __TORQUE_CONTROLLERS_DATA__ +#define __TORQUE_CONTROLLERS_DATA__ + +#include + +struct TCMuxStatus_s +{ + bool speedPreventsModeChange; + bool torqueDeltaPreventsModeChange; + bool controllerNotReadyPreventsModeChange; + bool steeringSystemError; + + uint8_t modeIntended; + uint8_t modeActual; + uint8_t dialMode; + + uint8_t torqueMode; + float maxTorque; +}; struct PIDTVTorqueControllerData { @@ -11,4 +28,4 @@ struct PIDTVTorqueControllerData float rr_torque_delta; }; -#endif \ No newline at end of file +#endif /* __TORQUE_CONTROLLERS_DATA__ */ \ No newline at end of file diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 38f914700..3a6db48c7 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -26,7 +26,24 @@ DrivetrainCommand_s CASESystem::evaluate( // in.YawRaterads = 2.5; // REAL - in.Vx_B = vn_data.velocity_x; + // if ( (vn_status < 2) || (vn_data.velocity_x < 0) ) + // { + // if + // in.Vx_B = 0; + // } else { + // in.Vx_B = vn_data.velocity_x; + // } + + if (vn_data.velocity_x < 0) + { + in.Vx_B = 0; + } else { + in.Vx_B = vn_data.velocity_x; + } + + + + in.TCSVelThreshold = config_.tcsVelThreshold; // FAKE // in.Vx_B = 5; @@ -62,6 +79,8 @@ DrivetrainCommand_s CASESystem::evaluate( in.usePIDPowerLimit = config_.usePIDPowerLimit; in.useLaunch = config_.useLaunch; + in.TCSVelThreshold = config_.tcsVelThreshold; + in.Vy_B = vn_data.velocity_y; in.YawPIDConfig[0] = config_.yaw_pid_p; @@ -164,14 +183,7 @@ DrivetrainCommand_s CASESystem::evaluate( in.useNL_TCS_SlipSchedule = config_.useNL_TCS_SlipSchedule; - if ((vn_active_start_time_ == 0) && (vn_status >= 2)) - { - vn_active_start_time_ = tick.millis; - } - else if (vn_status < 2) - { - vn_active_start_time_ = 0; - } + case_.setExternalInputs(&in); if ((tick.millis - last_eval_time_) >= 1) @@ -190,11 +202,12 @@ DrivetrainCommand_s CASESystem::evaluate( if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_)) { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); last_controller_pt1_send_time_ = tick.millis; } @@ -202,11 +215,10 @@ DrivetrainCommand_s CASESystem::evaluate( if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) && ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_)) { - - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn); @@ -217,12 +229,11 @@ DrivetrainCommand_s CASESystem::evaluate( ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_)) { - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p); - enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn); last_controller_pt3_send_time_ = tick.millis; } diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index c2fa7f7c5..3cec2e4a9 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -5,12 +5,14 @@ #include "AnalogSensorsInterface.h" #include "Filter_IIR.h" #include "SysClock.h" +#include "TelemetryInterface.h" + // Digital Encoder = Primary Sensor // Analog Encoder = Secondary Sensor // Definitions // TODO: evalaute reasonable thresholds for agreement -#define STEERING_DIVERGENCE_ERROR_THRESHOLD (16.0) // Steering sensors can disagree by x degrees before output is considered erroneous +#define STEERING_DIVERGENCE_ERROR_THRESHOLD (60.0) // Steering sensors can disagree by x degrees before output is considered erroneous #define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge x degrees #define NUM_SENSORS 2 #define DEFAULT_STEERING_ALPHA (0.0) @@ -52,17 +54,20 @@ class SteeringSystem Filter_IIR steeringFilters_[NUM_SENSORS]; float filteredAnglePrimary_; float filteredAngleSecondary_; + + TelemetryInterface *telemHandle_; public: - SteeringSystem(SteeringEncoderInterface *primarySensor) - : SteeringSystem(primarySensor, DEFAULT_STEERING_ALPHA) + SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface) + : SteeringSystem(primarySensor, telemInterface, DEFAULT_STEERING_ALPHA) {} - SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlpha) - : SteeringSystem(primarySensor, filterAlpha, filterAlpha) + SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface, float filterAlpha) + : SteeringSystem(primarySensor, telemInterface, filterAlpha, filterAlpha) {} - SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlphaPrimary, float filterAlphaSecondary) + SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface, float filterAlphaPrimary, float filterAlphaSecondary) : primarySensor_(primarySensor) + , telemHandle_(telemInterface) { steeringFilters_[0] = Filter_IIR(filterAlphaPrimary); steeringFilters_[1] = Filter_IIR(filterAlphaSecondary); @@ -80,6 +85,14 @@ class SteeringSystem { return steeringData_; } + + void reportSteeringStatus( + const float angle, + const float filteredAngleEncoder, + const float filteredAngleAnalog, + const uint8_t systemStatus, + const uint8_t encoderStatus, + const uint8_t analogSensorStatus); }; #endif /* __STEERINGSYSTEM_H__ */ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index cd8dc0501..f56731ba1 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -11,11 +11,12 @@ #include "DashboardInterface.h" #include "VectornavInterface.h" #include "LoadCellInterface.h" +#include "TelemetryInterface.h" const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm -/// @brief +/// @brief multiplexer class for managing available torque controllers class TorqueControllerMux { private: @@ -42,11 +43,11 @@ class TorqueControllerMux }; TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; - DialMode_e cur_dial_mode_ = DialMode_e::MODE_0; + DialMode_e currDialMode_ = DialMode_e::MODE_0; TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; - + // Handle array for all torque controllers TorqueControllerBase* controllers[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)] = { static_cast(&torqueControllerNone_), static_cast(&torqueControllerSimple_), @@ -56,33 +57,41 @@ class TorqueControllerMux static_cast(&tcCASEWrapper_) }; + // Status tracking structure for visibility + TCMuxStatus_s tcMuxStatus_; + DrivetrainCommand_s drivetrainCommand_; - TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE; + TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_FULL_TORQUE; bool torqueLimitButtonPressed_ = false; unsigned long torqueLimitButtonPressedTime_ = 0; + TelemetryInterface *telemHandle_; public: /// @brief torque controller mux in which default instances of all torque controllers are created for use - TorqueControllerMux() + TorqueControllerMux(TelemetryInterface *telemInterface) : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)]) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) - , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} + , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) + , telemHandle_(telemInterface) {} /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC /// @param simpleTCRearTorqueScale the scaling from 0 to 2 in which 2 is full rear torque allocation, 0 is full front, 1 = balanced /// @param simpleTCRegenTorqueScale scaling from 0 to 2 in which 0 is full rear regen and 2 is full front regen, 1 = balanced - TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale) + TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale, TelemetryInterface *telemInterface) : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) - , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} -// Functions + , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) + , telemHandle_(telemInterface) {} + + // Functions + /// @brief tick controllers to calculate drivetrain command void tick( const SysTick_s &tick, const DrivetrainDynamicReport_s &drivetrainData, @@ -90,16 +99,26 @@ class TorqueControllerMux const SteeringSystemData_s &steeringData, const LoadCellInterfaceOutput_s &loadCellData, DialMode_e dashboardDialMode, + float accDerateFactor, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, const DrivetrainCommand_s &CASECommand ); + + /// @brief apply corresponding limits on drivetrain command calculated by torque controller + void applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); + void applyTorqueLimit(DrivetrainCommand_s* command); + void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); + void applyPosSpeedLimit(DrivetrainCommand_s* command); + void applyDerate(DrivetrainCommand_s* command, float accDerateFactor); + + /// @brief GETTERS const DrivetrainCommand_s &getDrivetrainCommand() { return drivetrainCommand_; }; - const TorqueLimit_e &getTorqueLimit() + const TorqueLimit_e getTorqueLimit() { return torqueLimit_; }; @@ -109,25 +128,16 @@ class TorqueControllerMux return torqueLimitMap_[torqueLimit_]; } - void applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); - - void applyTorqueLimit(DrivetrainCommand_s* command); - - void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); - - void applyPosSpeedLimit(DrivetrainCommand_s* command); - - - const DialMode_e getDialMode() { - return cur_dial_mode_; + return currDialMode_; } const TorqueController_e getDriveMode() { return muxMode_; } + TorqueControllerBase* activeController() { @@ -139,6 +149,9 @@ class TorqueControllerMux return static_cast(&torqueControllerNone_); } } + + /// @brief report TCMux status through Telemetry via CAN + void reportTCMuxStatus(); }; #endif /* __TORQUECTRLMUX_H__ */ diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 18ed91e28..783d5350c 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -4,7 +4,6 @@ #include #include #include -#include #include "AnalogSensorsInterface.h" #include "DashboardInterface.h" #include "PhysicalParameters.h" @@ -35,7 +34,7 @@ const float MAX_REGEN_TORQUE = 10.0; /* LAUNCH CONSTANTS */ -const float DEFAULT_LAUNCH_RATE = 11.76; +const float DEFAULT_LAUNCH_RATE = 1.4 * 9.8; const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; const float DEFAULT_SLIP_RATIO = 0.2f; diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 32439736e..e074cf57d 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -8,64 +8,39 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) // 2. Computes internal state if (intake.tick.triggers.trigger100) { - // Poll upper steering sensor - primarySensor_->sample(); - primaryConversion_ = primarySensor_->convert(); - - // Filter both sensor angle readings - filteredAnglePrimary_ = steeringFilters_[0].filtered_result(primaryConversion_.angle); - filteredAngleSecondary_ = steeringFilters_[1].filtered_result(intake.secondaryConversion.conversion); - - // Both sensors are nominal and agree - if ( - (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) - && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD) - && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_WARN_THRESHOLD) - ) - { - steeringData_ = { - .angle = filteredAnglePrimary_, - .status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL - }; - } - // One or both sensors are marginal - // Sensors disagree by STEERING_DIVERGENCE_WARN_THRESHOLD degrees and less than STEERING_DIVERGENCE_ERROR_THRESHOLD degrees - else if ( - (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL) - || ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED)) - || ((std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_ERROR_THRESHOLD)) - ) - { - steeringData_ = { - .angle = filteredAnglePrimary_, - .status = SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL - }; - } - // Upper steering sensor reports error or is not detected, lower sensor is nominal - else if ( - (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_ERROR) - && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD) - ) - { - steeringData_ = { - .angle = filteredAngleSecondary_, - .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED - }; - } - // Fall through case - // Complete failure of steering sensing - else - { - steeringData_ = { - .angle = 0.0, - .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR - }; - } + filteredAngleSecondary_ = steeringFilters_[1].filtered_result(intake.secondaryConversion.conversion); + primaryConversion_.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR; + steeringData_.status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL; + steeringData_ = { + .angle = filteredAngleSecondary_, + .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED}; + } - // For possible bottom sensor recalibration - // TODO: Remove me once done! - // Serial.print("Secondary sensor raw: "); - // Serial.println(intake.secondaryConversion.raw); - // Serial.println(); + // Report at 50Hz + if (intake.tick.triggers.trigger50) + { + reportSteeringStatus( + steeringData_.angle, + 0, + filteredAngleSecondary_, + static_cast(steeringData_.status), + static_cast(primaryConversion_.status), + static_cast(intake.secondaryConversion.status)); } } + +void SteeringSystem::reportSteeringStatus(const float angle, + const float filteredAngleEncoder, + const float filteredAngleAnalog, + const uint8_t systemStatus, + const uint8_t encoderStatus, + const uint8_t analogSensorStatus) +{ + telemHandle_->update_steering_status_CAN_msg( + angle, + filteredAngleEncoder, + filteredAngleAnalog, + systemStatus, + encoderStatus, + analogSensorStatus); +} diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 13a84e9c6..e6e04fa8b 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -9,6 +9,7 @@ void TorqueControllerMux::tick( const SteeringSystemData_s &steeringData, const LoadCellInterfaceOutput_s &loadCellData, DialMode_e dashboardDialMode, + float accDerateFactor, bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, const DrivetrainCommand_s &CASECommand) @@ -27,8 +28,7 @@ void TorqueControllerMux::tick( if (tick.triggers.trigger50) { // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms - if ( - torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) + if (torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) { // WOW C++ is ass torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); @@ -74,8 +74,13 @@ void TorqueControllerMux::tick( if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) { muxMode_ = dialModeMap_[dashboardDialMode]; - cur_dial_mode_ = dashboardDialMode; + currDialMode_ = dashboardDialMode; } + + // Update TCMux status + tcMuxStatus_.speedPreventsModeChange = speedPreventsModeChange; + tcMuxStatus_.torqueDeltaPreventsModeChange = torqueDeltaPreventsModeChange; + tcMuxStatus_.controllerNotReadyPreventsModeChange = controllerNotReadyPreventsModeChange; } // Check if the current controller is ready. If it has faulted, revert to safe mode @@ -86,17 +91,49 @@ void TorqueControllerMux::tick( muxMode_ = TorqueController_e::TC_SAFE_MODE; } + // Update TCMux status + tcMuxStatus_.steeringSystemError = steeringData.status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR; + tcMuxStatus_.modeIntended = static_cast(dialModeMap_[dashboardDialMode]); + tcMuxStatus_.modeActual = static_cast(getDriveMode()); + tcMuxStatus_.dialMode = static_cast(getDialMode()); + drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; + // Update TCMux status + tcMuxStatus_.torqueMode = static_cast(getTorqueLimit()); + tcMuxStatus_.maxTorque = getMaxTorque(); + // Apply setpoints value limits - // Safety checks for CASE: CASE handles regen, torque, and power limit internally - applyRegenLimit(&drivetrainCommand_, &drivetrainData); - // Apply torque limit before power limit to not power limit - applyTorqueLimit(&drivetrainCommand_); - applyPowerLimit(&drivetrainCommand_, &drivetrainData); + // Derating for endurance + + + if (muxMode_ != TC_CASE_SYSTEM) + { + // Safety checks for CASE: CASE handles regen, torque, and power limit internally + applyRegenLimit(&drivetrainCommand_, &drivetrainData); + // Apply torque limit before power limit to not power limit + if ((muxMode_ != TC_SIMPLE_LAUNCH) && (muxMode_ != TC_SLIP_LAUNCH) && (muxMode_ != TC_LOOKUP_LAUNCH)) + { + applyTorqueLimit(&drivetrainCommand_); + } + + applyPowerLimit(&drivetrainCommand_, &drivetrainData); + } else { + applyDerate(&drivetrainCommand_, accDerateFactor); + } + // Uniformly apply speed limit to all controller modes applyPosSpeedLimit(&drivetrainCommand_); + + } + + // Update controller status report + if (tick.triggers.trigger50) + { + reportTCMuxStatus(); + } + } /* @@ -141,6 +178,24 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const Dr } } +/* + Apply derating factor + - Endurance +*/ +void TorqueControllerMux::applyDerate(DrivetrainCommand_s *command, float accDerateFactor) +{ + if (command->speeds_rpm[0] != 0 && + command->speeds_rpm[1] != 0 && + command->speeds_rpm[2] != 0 && + command->speeds_rpm[2] != 0) { + for (int i = 0; i < NUM_MOTORS; i++) + { + command->torqueSetpoints[i] *= accDerateFactor; + } + } + +} + /* Apply power limit such that the mechanical power of all wheels never exceeds the preset mechanical power limit. Scales all wheels down to @@ -224,6 +279,11 @@ void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s *command) command->torqueSetpoints[i] /= scale; } } + + for (int i = 0; i < NUM_MOTORS; i++) + { + command->torqueSetpoints[i] = std::min(command->torqueSetpoints[i], max_torque); + } } /** @@ -235,4 +295,21 @@ void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command) { command->speeds_rpm[i] = std::max(0.0f, command->speeds_rpm[i]); } +} + +/** + * Report TCMux status via CAN + * - speedPreventsModeChange + * - torqueDeltaPreventsModeChange + * - controllerNotReadyPreventsModeChange + * - steeringSystemError + * - modeIntended + * - modeActual + * - dialMode + * - torqueMode + * - maxTorque +*/ +void TorqueControllerMux::reportTCMuxStatus() +{ + telemHandle_->update_TCMux_status_CAN_msg(tcMuxStatus_); } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 4351c4dc8..8ed47cec9 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -127,18 +127,24 @@ void TorqueControllerLoadCellVectoring::tick( else { // Negative torque request - // No load cell vectoring on regen - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + + torquePool = MAX_REGEN_TORQUE * accelRequest * -4.0; writeout_.command.speeds_rpm[FL] = 0.0; writeout_.command.speeds_rpm[FR] = 0.0; writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.torqueSetpoints[FL] = torquePool * frontRegenTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; + writeout_.command.torqueSetpoints[FR] = torquePool * frontRegenTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; + writeout_.command.torqueSetpoints[RL] = torquePool * rearRegenTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; + writeout_.command.torqueSetpoints[RR] = torquePool * rearRegenTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; + + // No load cell vectoring on regen + // writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; + // writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; + // writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; + // writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; } } else diff --git a/platformio.ini b/platformio.ini index 5d9237b66..20b5a1b17 100644 --- a/platformio.ini +++ b/platformio.ini @@ -50,14 +50,15 @@ lib_deps = Nanopb https://github.com/vjmuzik/NativeEthernet.git https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz - https://github.com/hytech-racing/shared_firmware_interfaces.git#feature/parameterize-filter-class + 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/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/97/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/99/can_lib.tar.gz git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 + [env:test_can_on_teensy] lib_ignore = mock_interfaces @@ -86,5 +87,5 @@ lib_deps = https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#4ffbba4 https://github.com/hytech-racing/HT_CAN/releases/latest/download/can_lib.tar.gz - + diff --git a/src/main.cpp b/src/main.cpp index 8f2357000..f159b590b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,6 +11,7 @@ // /* Interfaces */ #include "HytechCANInterface.h" +#include "ThermistorInterface.h" #include "Teensy_ADC.h" #include "MCP_ADC.h" #include "ORBIS_BR10.h" @@ -115,10 +116,11 @@ ParameterInterface param_interface; ETHInterfaces ethernet_interfaces = {¶m_interface}; VNInterface vn_interface(&CAN3_txBuffer); DashboardInterface dashboard(&CAN3_txBuffer); -AMSInterface ams_interface(8); -WatchdogInterface wd_interface(32); +AMSInterface ams_interface(&CAN3_txBuffer, SOFTWARE_OK); +WatchdogInterface wd_interface(WATCHDOG_INPUT); MCUInterface main_ecu(&CAN3_txBuffer); TelemetryInterface telem_interface(&CAN3_txBuffer, telem_read_channels); +ThermistorInterface front_thermistors_interface(&CAN3_txBuffer); SABInterface sab_interface( LOADCELL_RL_SCALE, // RL Scale LOADCELL_RL_OFFSET, // RL Offset (Migos) @@ -142,7 +144,7 @@ struct inverters // */ SysClock sys_clock; -SteeringSystem steering_system(&steering1, STEERING_IIR_ALPHA); +SteeringSystem steering_system(&steering1, &telem_interface, STEERING_IIR_ALPHA); BuzzerController buzzer(BUZZER_ON_INTERVAL); SafetySystem safety_system(&ams_interface, &wd_interface); @@ -150,7 +152,7 @@ SafetySystem safety_system(&ams_interface, &wd_interface); PedalsSystem pedals_system(accel_params, brake_params); using DriveSys_t = DrivetrainSystem; DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); -TorqueControllerMux torque_controller_mux(1.0, 0.4); +TorqueControllerMux torque_controller_mux(SIMPLE_TC_REAR_TORQUE_SCALE, SIMPLE_TC_REGEN_TORQUE_SCALE, &telem_interface); // TODO ensure that case uses max regen torque, right now its not CASEConfiguration case_config = { // Following used for generated code @@ -159,9 +161,9 @@ CASEConfiguration case_config = { .yaw_pid_i = 0.25, .yaw_pid_d = 0.0, .tcs_pid_p_lowerBound_front = 55.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error - .tcs_pid_p_upperBound_front = 45.0, + .tcs_pid_p_upperBound_front = 42.0, .tcs_pid_p_lowerBound_rear = 32.0, - .tcs_pid_p_upperBound_rear = 45.0, + .tcs_pid_p_upperBound_rear = 42.0, .tcs_pid_i = 0.0, .tcs_pid_d = 0.0, .useLaunch = false, @@ -204,15 +206,15 @@ CASEConfiguration case_config = { .TCSGenLeftRightDiffUpperBound = 20, // N-m .TCSWheelSteerLowerBound = 2, // Deg .TCSWheelSteerUpperBound = 25, // Deg - .useRPM_TCS_GainSchedule = false, // If both are false, then P values defaults to lower bound per axle - .useNL_TCS_GainSchedule = true, + .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, .TCS_NL_startBoundPerc_RearAxle = 0.5, .TCS_NL_endBoundPerc_RearAxle = 0.6, .useNL_TCS_SlipSchedule = true, .launchSL_startBound_Front = 0.25, - .launchSL_endBound_Front = 0.15, + .launchSL_endBound_Front = 0.25, .launchSL_startBound_Rear = 0.3, .launchSL_endBound_Rear = 0.4, .TCS_SL_startBound_Front = 0.25, @@ -291,7 +293,7 @@ void setup() a1.setChannelOffset(MCU15_STEERING_CHANNEL, -1 * SECONDARY_STEERING_SENSE_CENTER); a1.setChannelScale(MCU15_STEERING_CHANNEL, STEERING_RANGE_DEGREES / ((float)SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND - (float)SECONDARY_STEERING_SENSE_LEFTMOST_BOUND)); a1.setChannelClamp(MCU15_STEERING_CHANNEL, -STEERING_RANGE_DEGREES / 0.5 * 1.15, STEERING_RANGE_DEGREES / 0.5 * 1.15); // 15% tolerance on each end of the steering sensor - + a1.setChannelScale(MCU15_GLV_SENSE_CHANNEL, GLV_SENSE_SCALE); a2.setChannelScale(MCU15_FL_LOADCELL_CHANNEL, LOADCELL_FL_SCALE /*Todo*/); a3.setChannelScale(MCU15_FR_LOADCELL_CHANNEL, LOADCELL_FR_SCALE /*Todo*/); @@ -309,7 +311,7 @@ void setup() main_ecu.init(); // initial shutdown circuit readings, wd_interface.init(curr_tick.millis); // initialize wd kick time - ams_interface.init(curr_tick.millis); // initialize last heartbeat time + ams_interface.init(curr_tick); // initialize last heartbeat time steering1.init(); steering1.setOffset(PRIMARY_STEERING_SENSE_OFFSET); @@ -381,6 +383,27 @@ void loop() Serial.print("Sensor divergence: "); Serial.println(steering1.convert().angle - a1.get().conversions[MCU15_STEERING_CHANNEL].conversion); Serial.println(); + Serial.println("Pedal outputs:"); + Serial.print("Accel 1 raw: "); + Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw); + Serial.print("Accel 2 raw: "); + Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw); + Serial.print("Accel percent: "); + Serial.println(pedals_system.getPedalsSystemDataCopy().accelPercent); + Serial.print("Brake 1 raw: "); + Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw); + Serial.print("Brake 2 raw: "); + Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw); + Serial.print("Brake percent: "); + Serial.println(pedals_system.getPedalsSystemDataCopy().brakePercent); + Serial.println(); + Serial.print("Derating factor: "); + Serial.println(ams_interface.get_acc_derate_factor()); + Serial.print("Filtered min cell voltage: "); + Serial.println(ams_interface.get_filtered_min_cell_voltage()); + Serial.print("Filtered max cell temp: "); + Serial.println(ams_interface.get_filtered_max_cell_temp()); + Serial.println(); Serial.println(); } @@ -429,7 +452,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) drivetrain.drivetrain_error_occured(), torque_controller_mux.getTorqueLimit(), ams_interface.get_filtered_min_cell_voltage(), - telem_interface.get_glv_voltage(a1.get()), + a1.get().conversions[MCU15_GLV_SENSE_CHANNEL], static_cast(torque_controller_mux.activeController()->get_launch_state()), dashboard.getDialMode()); @@ -446,12 +469,11 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) dashboard.launchControlButtonPressed()); 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]); telem_interface.tick( a1.get(), a2.get(), a3.get(), - mcu_adc.get(), steering1.convert(), &inv.fl, &inv.fr, @@ -472,6 +494,7 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) if (t.trigger50) // 50Hz { steering1.sample(); + ams_interface.tick(current_system_tick); } if (t.trigger100) // 100Hz @@ -505,7 +528,8 @@ void tick_all_systems(const SysTick_s ¤t_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], + a1.get().conversions[MCU15_BRAKE2_CHANNEL]); // tick steering system steering_system.tick( @@ -532,7 +556,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) 0, fsm.get_state(), dashboard.startButtonPressed(), - 3); + vn_interface.get_vn_struct().vn_status); // case_system.update_config_from_param_interface(param_interface); @@ -543,6 +567,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) steering_system.getSteeringSystemData(), load_cell_interface.getLoadCellForces(), dashboard.getDialMode(), + ams_interface.get_acc_derate_factor(), dashboard.torqueModeButtonPressed(), vn_interface.get_vn_struct(), controller_output); diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 8038eb36d..a1d174e6d 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -1,4 +1,5 @@ #include +#include #include "state_machine_test.h" // #include "pedals_system_test.h" @@ -7,7 +8,7 @@ #include "safety_system_test.h" // #include "test_CASE.h" // #include "param_system_test.h" -#include "steering_system_test.h" +// #include "steering_system_test.h" int main(int argc, char **argv) { diff --git a/test/test_systems/state_machine_test.h b/test/test_systems/state_machine_test.h index 74f312948..251ac6a03 100644 --- a/test/test_systems/state_machine_test.h +++ b/test/test_systems/state_machine_test.h @@ -65,7 +65,8 @@ TEST(MCUStateMachineTesting, test_state_machine_init_tick) DrivetrainMock drivetrain; PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; @@ -81,7 +82,8 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) DrivetrainMock drivetrain; PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; @@ -116,7 +118,8 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) DrivetrainMock drivetrain; PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); @@ -162,7 +165,8 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) DrivetrainMock drivetrain; PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); @@ -196,7 +200,8 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) DrivetrainMock drivetrain; PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); @@ -230,7 +235,8 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti drivetrain.drivetrain_error_ = false; PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); @@ -278,7 +284,8 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ PedalsSystem pedals({},{}); DashboardInterface dash_interface; - TorqueControllerMux tc_mux; + TelemetryInterface telem_interface; + TorqueControllerMux tc_mux(&telem_interface); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h index ff42060d0..38be1276e 100644 --- a/test/test_systems/steering_system_test.h +++ b/test/test_systems/steering_system_test.h @@ -18,7 +18,8 @@ TEST(SteeringSystemTesting, test_steering_nominal) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); // Filter refernce values for evaluation Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; @@ -51,7 +52,8 @@ TEST(SteeringSystemTesting, test_steering_primary_is_marginal) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); // Filter refernce values for evaluation Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float angle = 0.0f; @@ -80,7 +82,8 @@ TEST(SteeringSystemTesting, test_steering_secondary_is_marginal) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); // Filter refernce values for evaluation Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float angle = 0.0f; @@ -109,7 +112,8 @@ TEST(SteeringSystemTesting, test_steering_divergence_warning) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); // Filter refernce values for evaluation Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float mockPrimaryFilteredAngle; @@ -157,7 +161,8 @@ TEST(SteeringSystemTesting, test_steering_divergence_error) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); // Filter refernce values for evaluation Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float mockPrimaryFilteredAngle; @@ -205,7 +210,8 @@ TEST(SteeringSystemTesting, test_steering_primary_is_missing) SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); // Filter refernce values for evaluation Filter_IIR mockAngleFilters[2] = {PRIMARY_ALPHA, SECONDARY_ALPHA}; float angle = 100.0f; @@ -234,7 +240,8 @@ TEST(SteeringSystemTesting, test_steering_primary_is_missing_and_secondary_is_ma SysClock sys_clock; SysTick_s sys_tick = sys_clock.tick(0); SteeringEncoderInterface primarySensor; - SteeringSystem steeringSystem(&primarySensor, PRIMARY_ALPHA, SECONDARY_ALPHA); + TelemetryInterface telem_interface; + SteeringSystem steeringSystem(&primarySensor, &telem_interface, PRIMARY_ALPHA, SECONDARY_ALPHA); float angle = 100.0f; primarySensor.mockConversion.angle = 0.0f; diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index 7d6a0e4c2..64a63b5f0 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -67,7 +67,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) SysClock clock = SysClock(); SysTick_s cur_tick; cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); + TelemetryInterface telem_interface; + TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); DrivetrainCommand_s resulting_torque_command; vector_nav vn_data{}; @@ -109,6 +110,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -118,8 +120,8 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) for (int i = 0; i < NUM_MOTORS; i++) { - ASSERT_LE(resulting_torque_command.torqueSetpoints[i], 0.0001); - ASSERT_LE(resulting_torque_command.speeds_rpm[i], 0.0001); + ASSERT_LT(resulting_torque_command.torqueSetpoints[i], 0.0001); + ASSERT_LT(resulting_torque_command.speeds_rpm[i], 0.0001); } // Release the pedal. The mode should change now @@ -130,6 +132,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -143,6 +146,7 @@ TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -162,7 +166,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) SysClock clock = SysClock(); SysTick_s cur_tick; cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); + TelemetryInterface telem_interface; + TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); DrivetrainCommand_s resulting_torque_command; vector_nav vn_data{}; @@ -221,6 +226,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -242,6 +248,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -255,6 +262,7 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -270,7 +278,8 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) } TEST(TorqueControllerMuxTesting, test_power_limit) { - TorqueControllerMux mux = TorqueControllerMux(); + TelemetryInterface telem_interface; + TorqueControllerMux mux = TorqueControllerMux(&telem_interface); DrivetrainCommand_s drive_command; DrivetrainDynamicReport_s edit; @@ -304,7 +313,8 @@ TEST(TorqueControllerMuxTesting, test_power_limit) { } TEST(TorqueControllerMuxTesting, test_torque_limit) { - TorqueControllerMux mux = TorqueControllerMux(); + TelemetryInterface telem_interface; + TorqueControllerMux mux = TorqueControllerMux(&telem_interface); DrivetrainCommand_s drive_command; for (int i = 0; i < 4; i++) { @@ -329,10 +339,23 @@ TEST(TorqueControllerMuxTesting, test_torque_limit) { mux.applyTorqueLimit(&drive_command); - ASSERT_LT(drive_command.torqueSetpoints[0], 3.5f); - ASSERT_LT(drive_command.torqueSetpoints[1], 12.5f); - ASSERT_LT(drive_command.torqueSetpoints[2], 12.5f); - ASSERT_LT(drive_command.torqueSetpoints[3], 12.5f); + ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); + ASSERT_EQ(drive_command.torqueSetpoints[1], 20.0f); + ASSERT_EQ(drive_command.torqueSetpoints[2], 20.0f); + ASSERT_EQ(drive_command.torqueSetpoints[3], 20.0f); + + for (int i = 0; i < 4; i++) { + drive_command.speeds_rpm[i] = 500.0f; + drive_command.torqueSetpoints[i] = 30.0f; + } + drive_command.torqueSetpoints[0] = 5; + + mux.applyTorqueLimit(&drive_command); + + ASSERT_LT(drive_command.torqueSetpoints[0], 4.6f); + ASSERT_LT(drive_command.torqueSetpoints[1], 27.1f); + ASSERT_LT(drive_command.torqueSetpoints[2], 27.1f); + ASSERT_LT(drive_command.torqueSetpoints[3], 27.1f); printf("torque 1: %.2f\n", drive_command.torqueSetpoints[0]); printf("torque 2: %.2f\n", drive_command.torqueSetpoints[1]); @@ -344,7 +367,8 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { SysClock clock = SysClock(); SysTick_s cur_tick; cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); + TelemetryInterface telem_interface; + TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); DrivetrainCommand_s resulting_torque_command; vector_nav vn_data{}; @@ -428,6 +452,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -441,6 +466,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -454,6 +480,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -471,6 +498,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -488,6 +516,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -505,6 +534,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -521,6 +551,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -538,6 +569,7 @@ TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_3, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -552,7 +584,8 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { SysClock clock = SysClock(); SysTick_s cur_tick; cur_tick = clock.tick(0); - TorqueControllerMux torque_controller_mux = TorqueControllerMux(); + TelemetryInterface telem_interface; + TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); DrivetrainCommand_s resulting_torque_command; vector_nav vn_data{}; @@ -636,6 +669,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_0, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -649,6 +683,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -662,6 +697,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -679,6 +715,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -692,6 +729,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -711,6 +749,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -733,6 +772,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {} @@ -753,6 +793,7 @@ TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { (const SteeringSystemData_s) {}, (const LoadCellInterfaceOutput_s) {}, DialMode_e::MODE_4, + 1.0, false, vn_data, (const DrivetrainCommand_s) {}