diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 16ed0f851..a11ee99dc 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -2,9 +2,7 @@ #define __MCU15_H__ #include "PedalsSystem.h" -#ifndef TESTING_SYSTEMS -#include "InterfaceParams.h" -#endif + // pindefs @@ -54,17 +52,17 @@ const unsigned long TELEM_CAN_BAUDRATE = 500000; // All of these values are the PEDAL min/max // the sensor min/max that trip implaus are calculated // in the PedalsSystem constructor -const int ACCEL1_PEDAL_MAX = 3409; // 3330 -const int ACCEL2_PEDAL_MAX = 259; // 388 +const int ACCEL1_PEDAL_MAX = 3491; +const int ACCEL2_PEDAL_MAX = 189; -const int ACCEL1_PEDAL_MIN = 2129; -const int ACCEL2_PEDAL_MIN = 1517; +const int ACCEL1_PEDAL_MIN = 2244; +const int ACCEL2_PEDAL_MIN = 1405; -const int BRAKE1_PEDAL_MAX = 1945; // 2200; -const int BRAKE2_PEDAL_MAX = 1742; // 2200; +const int BRAKE1_PEDAL_MAX = 1945; +const int BRAKE2_PEDAL_MAX = 1742; -const int BRAKE1_PEDAL_MIN = 1230; // 785; // 1230 to 1750 -const int BRAKE2_PEDAL_MIN = 2437; // 785; // 2450 to 1930 +const int BRAKE1_PEDAL_MIN = 1192; +const int BRAKE2_PEDAL_MIN = 2476; const int ACCEL1_PEDAL_OOR_MIN = 90; const int ACCEL2_PEDAL_OOR_MIN = 90; diff --git a/include/PrintLogger.h b/include/PrintLogger.h new file mode 100644 index 000000000..fad214d1d --- /dev/null +++ b/include/PrintLogger.h @@ -0,0 +1,44 @@ +#ifndef __PRINTLOGGER_H__ +#define __PRINTLOGGER_H__ +#include +#include +#ifdef ARDUINO +#include +#else +#include +#include +#include +#endif + +#ifdef ARDUINO +template +void logger_println(T out){ + Serial.println(out); +} +#else +template +void logger_println(T out){ + std::cout << out< + void log_out(T out, unsigned long current_millis, unsigned long period=100) { + if((current_millis - last_out_time_) > period) + { + logger_println(out); + last_out_time_ = current_millis; + } + } +}; +#endif // __PRINTLOGGER_H__ \ No newline at end of file diff --git a/lib/interfaces/include/DashboardInterface.h b/lib/interfaces/include/DashboardInterface.h index 494e83384..082164dad 100644 --- a/lib/interfaces/include/DashboardInterface.h +++ b/lib/interfaces/include/DashboardInterface.h @@ -6,36 +6,9 @@ #include "hytech.h" #include "MCUInterface.h" #include "InverterInterface.h" +#include "SharedDataTypes.h" -#include "TorqueControllers.h" -/* - Enum for the car's torque limits - MOVE ME! - ideally into a TorqueControllerDefs.h file - to prevent circular dependencies -*/ -enum class TorqueLimit_e -{ - TCMUX_LOW_TORQUE = 0, - TCMUX_MID_TORQUE = 1, - TCMUX_FULL_TORQUE = 2, - TCMUX_NUM_TORQUE_LIMITS = 3, -}; - -/* Enum for the modes on the dial, corresponds directly to dial index pos. */ -enum class DialMode_e -{ - /* No torque vectoring */ - MODE_0, - /* Normal force torque vectoring */ - MODE_1, - /* PID torque vectoring */ - MODE_2, - /* Launch Ctrl */ - MODE_3, - MODE_4, - MODE_5, -}; /* Enum for defined LED colors. ON will be LED's default color on dashboard*/ enum class LEDColors_e @@ -77,13 +50,15 @@ struct DashButtons_s bool right_shifter; }; + /* Struct holding all data for the DashboardInterface (inputs and outputs) */ struct DashComponentInterface_s { /* READ DATA */ // enum for dial position read by controller mux - DialMode_e dial_mode; - DialMode_e cur_dial_mode; + ControllerMode_e dial_mode; + ControllerMode_e cur_dial_mode; + TorqueLimit_e torque_limit_mode; // Buttons struct for better naming DashButtons_s button; bool ssok; // safety system OK (IMD?) RENAME @@ -111,6 +86,8 @@ class DashboardInterface CANBufferType *msg_queue_; /* The instantiated data struct used to access data by member functions */ DashComponentInterface_s _data; + bool prev_button_pressed_state_; + void update_torque_mode_(bool button_pressed); public: /*! @@ -121,6 +98,8 @@ class DashboardInterface */ DashboardInterface(CANBufferType *msg_output_queue) { + _data.torque_limit_mode = TorqueLimit_e::TCMUX_FULL_TORQUE; + prev_button_pressed_state_ = false; msg_queue_ = msg_output_queue; }; @@ -145,14 +124,14 @@ class DashboardInterface float min_cell_voltage, AnalogConversion_s glv_voltage, int launch_state, - DialMode_e dial_mode); + ControllerMode_e dial_mode); /*! getter for the dashboard's current dial position (drive profile) - @return returns a DialMode_e enum with the current dial position + @return returns a ControllerMode_e enum with the current dial position */ - DialMode_e getDialMode(); - + ControllerMode_e getDialMode(); + TorqueLimit_e getTorqueLimitMode(); /* gets whether the safety system is ok (wtf is a safety system - rename this)*/ bool safetySystemOK(); diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 7e1c47235..77a250cc1 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -170,7 +170,7 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case VN_LAT_LON_CANID: interfaces.vn_interface->retrieve_lat_lon_CAN(recvd_msg); break; - case VN_GPS_TIME_CANID: + case VN_GPS_TIME_MSG_CANID: interfaces.vn_interface->retrieve_gps_time_CAN(recvd_msg); break; case VN_STATUS_CANID: diff --git a/lib/interfaces/include/LoadCellInterface.h b/lib/interfaces/include/LoadCellInterface.h index adf85befb..7d5dc4d0c 100644 --- a/lib/interfaces/include/LoadCellInterface.h +++ b/lib/interfaces/include/LoadCellInterface.h @@ -4,6 +4,7 @@ #include "Utility.h" #include "SysClock.h" #include "AnalogSensorsInterface.h" +#include "SharedDataTypes.h" /* Structs */ @@ -15,12 +16,6 @@ struct LoadCellInterfaceTick_s const AnalogConversion_s &RRConversion; }; -struct LoadCellInterfaceOutput_s -{ - veh_vec loadCellForcesFiltered; - veh_vec loadCellConversions; - bool FIRSaturated; -}; class LoadCellInterface { diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index 521f10301..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/include/ParameterInterface.h b/lib/interfaces/include/ParameterInterface.h deleted file mode 100644 index c6b0cac1c..000000000 --- a/lib/interfaces/include/ParameterInterface.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef PARAMETERINTERFACE -#define PARAMETERINTERFACE -#include "MCUStateMachine.h" -#include "ht_eth.pb.h" -#include "default_config.h" - -// yes, i know this is a singleton. im prototyping rn. -// TODO review if I can just give this a pointer to an ethernet port -class ParameterInterface -{ -public: - ParameterInterface(): current_car_state_(CAR_STATE::STARTUP), params_need_sending_(false), config_(DEFAULT_CONFIG) {} - - void update_car_state(const CAR_STATE& state) - { - current_car_state_ = state; - } - void update_config(const config &config) - { - if(static_cast(current_car_state_) < 5 ){ - config_ = config; - } - - } - config get_config() - { - return config_; - } - void set_params_need_sending() - { - params_need_sending_ = true; - } - void reset_params_need_sending() - { - params_need_sending_ = false; - } - bool params_need_sending() { return params_need_sending_; } - -private: - CAR_STATE current_car_state_; - bool params_need_sending_ = false; - config config_; - -}; - -#endif \ No newline at end of file diff --git a/lib/interfaces/include/ProtobufMsgInterface.h b/lib/interfaces/include/ProtobufMsgInterface.h index ffa4e469d..05e660442 100644 --- a/lib/interfaces/include/ProtobufMsgInterface.h +++ b/lib/interfaces/include/ProtobufMsgInterface.h @@ -1,19 +1,16 @@ #ifndef PROTOBUFMSGINTERFACE #define PROTOBUFMSGINTERFACE -#include "ht_eth.pb.h" #include "pb_encode.h" #include "pb_decode.h" #include "pb_common.h" -#include "ParameterInterface.h" #include "circular_buffer.h" #include "NativeEthernet.h" #include "MCU_rev15_defs.h" - +#include "InterfaceParams.h" struct ETHInterfaces { - ParameterInterface* param_interface; }; using recv_function_t = void (*)(const uint8_t* buffer, size_t packet_size, ETHInterfaces& interfaces); @@ -50,27 +47,17 @@ bool handle_ethernet_socket_send_pb(EthernetUDP* socket, const pb_struct& msg, c return true; } -// -void recv_pb_stream_union_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces) + +template +std::pair recv_pb_stream_msg(const uint8_t *buffer, size_t packet_size, ETHInterfaces& interfaces, const pb_msgdesc_t * desc_pointer) { pb_istream_t stream = pb_istream_from_buffer(buffer, packet_size); - HT_ETH_Union msg = HT_ETH_Union_init_zero; - if (pb_decode(&stream, HT_ETH_Union_fields, &msg)) + pb_msg_type msg = {}; + if (pb_decode(&stream, desc_pointer, &msg)) { - Serial.println("decoded!"); - - switch (msg.which_type_union) - { - case HT_ETH_Union_config__tag: - interfaces.param_interface->update_config(msg.type_union.config_); - break; - case HT_ETH_Union_get_config__tag: - interfaces.param_interface->set_params_need_sending(); - break; - default: - break; - } + return {msg, true}; } + return {msg, }; } diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index c9b360eb9..e8e028837 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -10,7 +10,8 @@ #include "SteeringEncoderInterface.h" #include "hytech.h" #include "InverterInterface.h" -#include "TorqueControllersData.h" +// #include "TorqueControllersData.h" +#include "SharedDataTypes.h" using InvInt_t = InverterInterface; const int FIXED_POINT_PRECISION = 1000; @@ -59,39 +60,33 @@ class TelemetryInterface void update_pedal_readings_CAN_msg( float accel_percent, float brake_percent, - float mech_brake_percent - ); + float mech_brake_percent); void update_pedal_readings_raw_CAN_msg( const AnalogConversion_s &accel_1, const AnalogConversion_s &accel_2, const AnalogConversion_s &brake_1, - const AnalogConversion_s &brake_2 - ); + const AnalogConversion_s &brake_2); void update_suspension_CAN_msg( const AnalogConversion_s &lc_fl, const AnalogConversion_s &lc_fr, const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr - ); + const AnalogConversion_s &pots_fr); 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,39 +95,29 @@ 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 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 - ); + const AnalogConversion_s &reference); - /* Enqueue outbound telemetry CAN messages */ - template - void enqueue_CAN(T can_msg, uint32_t id); + 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 */ + 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( @@ -153,7 +138,7 @@ class TelemetryInterface const AnalogConversion_s &brake_1, const AnalogConversion_s &brake_2, float mech_brake_active_percent, - const PIDTVTorqueControllerData &data); + const TorqueControllerMuxError ¤t_mux_status); }; #endif /* TELEMETRYINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/include/ThermistorInterface.h b/lib/interfaces/include/ThermistorInterface.h index 4c49f3ec6..4af4c5973 100644 --- a/lib/interfaces/include/ThermistorInterface.h +++ b/lib/interfaces/include/ThermistorInterface.h @@ -16,7 +16,7 @@ const int MCU15_THERM_FR_CHANNEL = 1; class ThermistorInterface { private: - Thermistors front_thermistors; + Thermistors _front_thermistors; float therm_fl; float therm_fr; diff --git a/lib/interfaces/include/VectornavInterface.h b/lib/interfaces/include/VectornavInterface.h index edf525cd9..d288e95bb 100644 --- a/lib/interfaces/include/VectornavInterface.h +++ b/lib/interfaces/include/VectornavInterface.h @@ -2,25 +2,7 @@ #define VECTORNAVINTERFACE #include "FlexCAN_T4.h" #include "hytech.h" - -struct vector_nav { - float velocity_x; - float velocity_y; - float velocity_z; - float linear_accel_x; - float linear_accel_y; - float linear_accel_z; - float uncompLinear_accel[3]; // 3D uncompensated linear acceleration - float yaw; - float pitch; - float roll; - double latitude; - double longitude; - double ecef_coords[3]; // x,y,z - uint64_t gps_time; // gps time - uint8_t vn_status; // status - xyz_vec angular_rates; - }; +#include "SharedDataTypes.h" template class VNInterface @@ -29,7 +11,7 @@ class VNInterface /* Watchdog last kicked time */ message_queue *msg_queue_; uint32_t can_id_; - vector_nav vn_data; + VectornavData_s vn_data; public: VNInterface(message_queue *msg_output_queue) { @@ -51,7 +33,7 @@ class VNInterface void retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg); void receive_ang_rates_CAN(CAN_message_t &recvd_msg); // getters - vector_nav get_vn_struct(); + VectornavData_s get_vn_struct(); uint32_t get_id() { return can_id_;}; }; diff --git a/lib/interfaces/include/VectornavInterface.tpp b/lib/interfaces/include/VectornavInterface.tpp index be07a4e08..3f081a077 100644 --- a/lib/interfaces/include/VectornavInterface.tpp +++ b/lib/interfaces/include/VectornavInterface.tpp @@ -50,8 +50,8 @@ void VNInterface::retrieve_lat_lon_CAN(CAN_message_t &recvd_msg) template void VNInterface::retrieve_gps_time_CAN(CAN_message_t &recvd_msg) { - VN_GPS_TIME_t gps_time_data; - Unpack_VN_GPS_TIME_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len); + VN_GPS_TIME_MSG_t gps_time_data; + Unpack_VN_GPS_TIME_MSG_hytech(&gps_time_data, recvd_msg.buf, recvd_msg.len); vn_data.gps_time = gps_time_data.vn_gps_time; } @@ -72,8 +72,8 @@ void VNInterface::retrieve_vn_ecef_pos_xy_CAN(CAN_message_t &recv template void VNInterface::retrieve_vn_ecef_pos_z_CAN(CAN_message_t &recvd_msg) { - VN_ECEF_POS_Z_t ecef_z; - Unpack_VN_ECEF_POS_Z_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); + VN_ECEF_POS_Z_MSG_t ecef_z; + Unpack_VN_ECEF_POS_Z_MSG_hytech(&ecef_z, recvd_msg.buf, recvd_msg.len); vn_data.ecef_coords[2] = HYTECH_vn_ecef_pos_z_ro_fromS(ecef_z.vn_ecef_pos_z_ro); } @@ -90,9 +90,9 @@ void VNInterface::receive_ang_rates_CAN(CAN_message_t & recvd_msg * @brief * getter method for returning vn_data structure * @tparam message_queue - * @return vector_nav + * @return VectornavData_s */ template -vector_nav VNInterface::get_vn_struct() { +VectornavData_s VNInterface::get_vn_struct() { return vn_data; } diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index de22b7c1b..0ae10472f 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -6,8 +6,8 @@ void DashboardInterface::read(const CAN_message_t &can_msg) DASHBOARD_STATE_t dash_state; Unpack_DASHBOARD_STATE_hytech(&dash_state, can_msg.buf, can_msg.len); - _data.dial_mode = static_cast(dash_state.dial_state); - + _data.dial_mode = static_cast(dash_state.dial_state); + _data.ssok = dash_state.ssok_above_threshold; _data.shutdown = dash_state.shutdown_h_above_threshold; @@ -23,10 +23,23 @@ void DashboardInterface::read(const CAN_message_t &can_msg) _data.buzzer_state = dash_state.drive_buzzer; + update_torque_mode_(_data.button.torque_mode); +} + +// TODO this should be part of the CAN message not in this interface. +void DashboardInterface::update_torque_mode_(bool button_pressed) +{ + // detect high-to-low transition + if (prev_button_pressed_state_ == true && button_pressed == false) + { + + _data.torque_limit_mode = static_cast((static_cast(_data.torque_limit_mode) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); + } + prev_button_pressed_state_ = button_pressed; } CAN_message_t DashboardInterface::write() -{ +{ DASHBOARD_MCU_STATE_t dash_mcu_state; dash_mcu_state.drive_buzzer = _data.buzzer_cmd; @@ -45,12 +58,12 @@ CAN_message_t DashboardInterface::write() dash_mcu_state.bots_led = _data.LED[static_cast(DashLED_e::BOTS_LED)]; dash_mcu_state.imd_led = _data.LED[static_cast(DashLED_e::IMD_LED)]; dash_mcu_state.ams_led = _data.LED[static_cast(DashLED_e::AMS_LED)]; - + dash_mcu_state.glv_led = _data.LED[static_cast(DashLED_e::GLV_LED)]; dash_mcu_state.pack_charge_led = _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)]; - + CAN_message_t can_msg; - auto id = Pack_DASHBOARD_MCU_STATE_hytech(&dash_mcu_state, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + auto id = Pack_DASHBOARD_MCU_STATE_hytech(&dash_mcu_state, can_msg.buf, &can_msg.len, (uint8_t *)&can_msg.flags.extended); can_msg.id = id; // this circular buffer implementation requires that you push your data in a array buffer // all this does is put the msg into a uint8_t buffer and pushes it onto the queue @@ -59,29 +72,28 @@ CAN_message_t DashboardInterface::write() msg_queue_->push_back(buf, sizeof(CAN_message_t)); return can_msg; - } -//figure out how to set enumed led colors or send (0,255 value) +// figure out how to set enumed led colors or send (0,255 value) void DashboardInterface::setLED(DashLED_e led, LEDColors_e color) { _data.LED[static_cast(led)] = static_cast(color); } -void DashboardInterface::tick10(MCUInterface* mcu, - int car_state, - bool buzzer, - bool drivetrain_error, +void DashboardInterface::tick10(MCUInterface *mcu, + int car_state, + bool buzzer, + bool drivetrain_error, TorqueLimit_e torque, float min_cell_voltage, AnalogConversion_s glv_voltage, int launch_state, - DialMode_e dial_mode) + ControllerMode_e dial_mode) { + // TODO unfuck this _data.cur_dial_mode = dial_mode; - soundBuzzer(buzzer); setLED(DashLED_e::AMS_LED, mcu->bms_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); @@ -91,7 +103,8 @@ void DashboardInterface::tick10(MCUInterface* mcu, setLED(DashLED_e::MC_ERROR_LED, !drivetrain_error ? LEDColors_e::ON : LEDColors_e::RED); setLED(DashLED_e::COCKPIT_BRB_LED, mcu->brb_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); - switch(launch_state){ + switch (launch_state) + { case 1: setLED(DashLED_e::LAUNCH_CONTROL_LED, LEDColors_e::RED); break; @@ -106,7 +119,8 @@ void DashboardInterface::tick10(MCUInterface* mcu, break; } - switch(torque){ + switch (torque) + { case TorqueLimit_e::TCMUX_LOW_TORQUE: setLED(DashLED_e::MODE_LED, LEDColors_e::OFF); break; @@ -121,27 +135,26 @@ void DashboardInterface::tick10(MCUInterface* mcu, break; } - uint16_t scaled_cell_voltage = (uint16_t)map((uint32_t)(min_cell_voltage*1000), 3300, 4200, 0, 255);// scale voltage - _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)] = std::max(0, std::min((int)scaled_cell_voltage, 255));// clamp voltage + uint16_t scaled_cell_voltage = (uint16_t)map((uint32_t)(min_cell_voltage * 1000), 3300, 4200, 0, 255); // scale voltage + _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)] = std::max(0, std::min((int)scaled_cell_voltage, 255)); // clamp voltage // _data.LED[DashLED_e::GLV_LED] = (uint8_t)map(glv_voltage.raw) write(); } -DialMode_e DashboardInterface::getDialMode() {return _data.dial_mode;} - -bool DashboardInterface::startButtonPressed() {return _data.button.start;} -bool DashboardInterface::specialButtonPressed() {return _data.button.mark;} -bool DashboardInterface::torqueModeButtonPressed() {return _data.button.torque_mode;} -bool DashboardInterface::inverterResetButtonPressed() {return _data.button.mc_cycle;} -bool DashboardInterface::launchControlButtonPressed() {return _data.button.launch_ctrl;} -bool DashboardInterface::nightModeButtonPressed() {return _data.button.led_dimmer;} -bool DashboardInterface::leftShifterButtonPressed() {return _data.button.left_shifter;} -bool DashboardInterface::rightShifterButtonPressed() {return _data.button.right_shifter;} - -bool DashboardInterface::safetySystemOK() {return _data.ssok;} -bool DashboardInterface::shutdownHAboveThreshold() {return _data.shutdown;} - -void DashboardInterface::soundBuzzer(bool state) {_data.buzzer_cmd = state;} -bool DashboardInterface::checkBuzzer() {return _data.buzzer_state;} - +ControllerMode_e DashboardInterface::getDialMode() { return _data.dial_mode; } +TorqueLimit_e DashboardInterface::getTorqueLimitMode() {return _data.torque_limit_mode; } +bool DashboardInterface::startButtonPressed() { return _data.button.start; } +bool DashboardInterface::specialButtonPressed() { return _data.button.mark; } +bool DashboardInterface::torqueModeButtonPressed() { return _data.button.torque_mode; } +bool DashboardInterface::inverterResetButtonPressed() { return _data.button.mc_cycle; } +bool DashboardInterface::launchControlButtonPressed() { return _data.button.launch_ctrl; } +bool DashboardInterface::nightModeButtonPressed() { return _data.button.led_dimmer; } +bool DashboardInterface::leftShifterButtonPressed() { return _data.button.left_shifter; } +bool DashboardInterface::rightShifterButtonPressed() { return _data.button.right_shifter; } + +bool DashboardInterface::safetySystemOK() { return _data.ssok; } +bool DashboardInterface::shutdownHAboveThreshold() { return _data.shutdown; } + +void DashboardInterface::soundBuzzer(bool state) { _data.buzzer_cmd = state; } +bool DashboardInterface::checkBuzzer() { return _data.buzzer_state; } diff --git a/lib/interfaces/src/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/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index b6c9ff0fb..e96a0b6cf 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -4,20 +4,22 @@ // MCP3208 returns structure void TelemetryInterface::update_pedal_readings_CAN_msg(float accel_percent, float brake_percent, - float mech_brake_percent) { + float mech_brake_percent) +{ MCU_PEDAL_READINGS_t pedal_readings; - pedal_readings.accel_percent_float_ro = HYTECH_accel_percent_float_ro_toS(accel_percent*100); - pedal_readings.brake_percent_float_ro = HYTECH_brake_percent_float_ro_toS(brake_percent*100); - pedal_readings.mechanical_brake_percent_float_ro = HYTECH_mechanical_brake_percent_float_ro_toS(mech_brake_percent*100); + pedal_readings.accel_percent_float_ro = HYTECH_accel_percent_float_ro_toS(accel_percent * 100); + pedal_readings.brake_percent_float_ro = HYTECH_brake_percent_float_ro_toS(brake_percent * 100); + pedal_readings.mechanical_brake_percent_float_ro = HYTECH_mechanical_brake_percent_float_ro_toS(mech_brake_percent * 100); enqueue_new_CAN(&pedal_readings, &Pack_MCU_PEDAL_READINGS_hytech); } void TelemetryInterface::update_pedal_readings_raw_CAN_msg(const AnalogConversion_s &accel_1, const AnalogConversion_s &accel_2, const AnalogConversion_s &brake_1, - const AnalogConversion_s &brake_2) { + const AnalogConversion_s &brake_2) +{ MCU_PEDAL_RAW_t pedal_read; pedal_read.accel_1_raw = accel_1.raw; @@ -26,13 +28,13 @@ void TelemetryInterface::update_pedal_readings_raw_CAN_msg(const AnalogConversio pedal_read.brake_2_raw = brake_2.raw; enqueue_new_CAN(&pedal_read, &Pack_MCU_PEDAL_RAW_hytech); - } // MCP3204 returns structure void TelemetryInterface::update_suspension_CAN_msg(const AnalogConversion_s &lc_fl, const AnalogConversion_s &lc_fr, const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr) { + const AnalogConversion_s &pots_fr) +{ MCU_SUSPENSION_t sus; sus.load_cell_fl = lc_fl.raw; sus.load_cell_fr = lc_fr.raw; @@ -46,7 +48,8 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon const AnalogConversion_s &steer2, const AnalogConversion_s ¤t, const AnalogConversion_s &reference, - const AnalogConversion_s &glv) { + const AnalogConversion_s &glv) +{ // do sth with mcu_analog_readings_ mcu_analog_readings_.set_steering_1(steer1.raw); mcu_analog_readings_.set_steering_2(steer2.raw); @@ -56,22 +59,22 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon enqueue_CAN(mcu_analog_readings_, ID_MCU_ANALOG_READINGS); } - -void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) +void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, InvInt_t *rr) { DRIVETRAIN_RPMS_TELEM_t rpms; rpms.fl_motor_rpm = fl->get_speed(); rpms.fr_motor_rpm = fr->get_speed(); rpms.rl_motor_rpm = rl->get_speed(); rpms.rr_motor_rpm = rr->get_speed(); - + enqueue_new_CAN(&rpms, &Pack_DRIVETRAIN_RPMS_TELEM_hytech); } -void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) +void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, InvInt_t *rr) { - if (1) { // We should only write error status if some error has occurred, but for now, this is just an if(1) + if (1) + { // We should only write error status if some error has occurred, but for now, this is just an if(1) DRIVETRAIN_ERR_STATUS_TELEM_t errors; errors.mc1_diagnostic_number = fl->get_error_status(); errors.mc2_diagnostic_number = fr->get_error_status(); @@ -79,18 +82,17 @@ void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t* fl, InvI errors.mc4_diagnostic_number = rr->get_error_status(); enqueue_new_CAN(&errors, &Pack_DRIVETRAIN_ERR_STATUS_TELEM_hytech); } - } void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( - InvInt_t* fl, - InvInt_t* fr, - InvInt_t* rl, - InvInt_t* rr, - bool accel_implaus, - bool brake_implaus, - float accel_per, - float brake_per) + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr, + bool accel_implaus, + bool brake_implaus, + float accel_per, + float brake_per) { DRIVETRAIN_STATUS_TELEM_t status; @@ -139,7 +141,7 @@ void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( MC_temps mc2_temps = fr->get_temps_msg(); MC_temps mc3_temps = rl->get_temps_msg(); MC_temps mc4_temps = rr->get_temps_msg(); - + MC_setpoints_command mc1_cmd = fl->get_cmd_msg(); MC_setpoints_command mc2_cmd = fr->get_cmd_msg(); MC_setpoints_command mc3_cmd = rl->get_cmd_msg(); @@ -157,18 +159,18 @@ void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( } void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg( - InvInt_t* fl, - InvInt_t* fr, - InvInt_t* rl, - InvInt_t* rr) + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr) { // TODO: change this to use actual torque values from inverter // Torque current just temporary for gearbox seal validation DRIVETRAIN_TORQUE_TELEM_t torque; - torque.fl_motor_torque_ro = HYTECH_fl_motor_torque_ro_toS(fl->get_torque_current()); - torque.fr_motor_torque_ro = HYTECH_fr_motor_torque_ro_toS(fr->get_torque_current()); - torque.rl_motor_torque_ro = HYTECH_rl_motor_torque_ro_toS(rl->get_torque_current()); - torque.rr_motor_torque_ro = HYTECH_rr_motor_torque_ro_toS(rr->get_torque_current()); + torque.fl_motor_torque = fl->get_torque_current(); + torque.fr_motor_torque = fr->get_torque_current(); + torque.rl_motor_torque = rl->get_torque_current(); + torque.rr_motor_torque = rr->get_torque_current(); enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech); } @@ -183,23 +185,6 @@ void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s enqueue_new_CAN(&message, &Pack_PENTHOUSE_ACCUM_MSG_hytech); } -void TelemetryInterface::update_TCMux_status_CAN_msg(const TCMuxStatus_s &tcMuxStatus) -{ - TCMUX_STATUS_REPORT_t msg; - - msg.speed_above_thresh = tcMuxStatus.speedPreventsModeChange; - msg.torque_delta_above_thresh = tcMuxStatus.torqueDeltaPreventsModeChange; - msg.tc_not_ready = tcMuxStatus.controllerNotReadyPreventsModeChange; - msg.steering_system_has_err = tcMuxStatus.steeringSystemError; - msg.mode_intended = tcMuxStatus.modeIntended; - msg.mode_actual = tcMuxStatus.modeActual; - msg.dash_dial_mode = tcMuxStatus.dialMode; - msg.torque_mode = tcMuxStatus.torqueMode; - msg.torque_limit_ro = HYTECH_torque_limit_ro_toS(tcMuxStatus.maxTorque); - - enqueue_new_CAN(&msg, &Pack_TCMUX_STATUS_REPORT_hytech); -} - void TelemetryInterface::update_steering_status_CAN_msg(const float steering_system_angle, const float filtered_angle_encoder, const float filtered_angle_analog, @@ -220,9 +205,10 @@ void TelemetryInterface::update_steering_status_CAN_msg(const float steering_sys } /* Send CAN messages */ -template -void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { - +template +void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) +{ + CAN_message_t msg; msg_class.write(msg.buf); msg.id = id; @@ -231,46 +217,29 @@ void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { memmove(buf, &msg, sizeof(CAN_message_t)); msg_queue_->push_back(buf, sizeof(CAN_message_t)); - } /* Send inverter CAN messages with new CAN library */ -template -void TelemetryInterface::enqueue_new_CAN(U* structure, uint32_t (* pack_function)(U*, uint8_t*, uint8_t*, uint8_t*)) { +template +void TelemetryInterface::enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *)) +{ CAN_message_t can_msg; - can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t *)&can_msg.flags.extended); uint8_t buf[sizeof(CAN_message_t)] = {}; memmove(buf, &can_msg, sizeof(CAN_message_t)); msg_queue_->push_back(buf, sizeof(CAN_message_t)); } -void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerData& data) -{ - CONTROLLER_PID_TV_DATA_t msg; - msg.controller_input_ro = HYTECH_controller_input_ro_toS(data.controller_input); - msg.controller_output_ro = HYTECH_controller_output_ro_toS(data.controller_output); - enqueue_new_CAN(&msg, &Pack_CONTROLLER_PID_TV_DATA_hytech); - - CONTROLLER_PID_TV_DELTA_DATA_t delta_msg; - delta_msg.pid_tv_fl_delta_ro = HYTECH_pid_tv_fl_delta_ro_toS(data.fl_torque_delta); - delta_msg.pid_tv_fr_delta_ro = HYTECH_pid_tv_fr_delta_ro_toS(data.fr_torque_delta); - delta_msg.pid_tv_rl_delta_ro = HYTECH_pid_tv_rl_delta_ro_toS(data.rl_torque_delta); - delta_msg.pid_tv_rr_delta_ro = HYTECH_pid_tv_rr_delta_ro_toS(data.rr_torque_delta); - - enqueue_new_CAN(&delta_msg, &Pack_CONTROLLER_PID_TV_DELTA_DATA_hytech); - - -} /* Tick SysClock */ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, const SteeringEncoderConversion_s &encoder, - InvInt_t* fl, - InvInt_t* fr, - InvInt_t* rl, - InvInt_t* rr, + InvInt_t *fl, + InvInt_t *fr, + InvInt_t *rl, + InvInt_t *rr, bool accel_implaus, bool brake_implaus, float accel_per, @@ -280,17 +249,22 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversion_s &brake_1, const AnalogConversion_s &brake_2, float mech_brake_active_percent, - const PIDTVTorqueControllerData& data) { + const TorqueControllerMuxError ¤t_mux_status) +{ + + MCU_ERROR_STATES_t error_states; + error_states.torque_controller_mux_status = static_cast(current_mux_status); + enqueue_new_CAN(&error_states, &Pack_MCU_ERROR_STATES_hytech); // Pedals update_pedal_readings_CAN_msg(accel_per, brake_per, mech_brake_active_percent); - + update_pedal_readings_raw_CAN_msg(accel_1, accel_2, brake_1, - brake_2); + brake_2); // Analog readings update_analog_readings_CAN_msg(encoder, adc1.conversions[channels_.analog_steering_channel], @@ -311,6 +285,4 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel], adc1.conversions[channels_.current_ref_channel]); - enqeue_controller_CAN_msg(data); - } \ No newline at end of file diff --git a/lib/interfaces/src/ThermistorInterface.cpp b/lib/interfaces/src/ThermistorInterface.cpp index 76f8ce6fa..29ac6d267 100644 --- a/lib/interfaces/src/ThermistorInterface.cpp +++ b/lib/interfaces/src/ThermistorInterface.cpp @@ -3,22 +3,21 @@ -ThermistorInterface::ThermistorInterface(CANBufferType *msg_output_queue) : front_thermistors() +ThermistorInterface::ThermistorInterface(CANBufferType *msg_output_queue) { _msg_queue = msg_output_queue; } - void ThermistorInterface::update_front_thermistor_readings() { - FRONT_THERMISTORS_t front_thermistors_; + 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); + front_thermistors.thermistor_motor_fl_ro = HYTECH_thermistor_motor_fl_ro_toS(therm_fl); + front_thermistors.thermistor_motor_fr_ro = HYTECH_thermistor_motor_fr_ro_toS(therm_fr); - enqueue_CAN_front_thermistors(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); + enqueue_CAN_front_thermistors(&front_thermistors, &Pack_FRONT_THERMISTORS_hytech); } template @@ -32,9 +31,13 @@ void ThermistorInterface::enqueue_CAN_front_thermistors(U* structure, uint32_t ( } void ThermistorInterface::tick(const AnalogConversion_s &raw_therm_fl, const AnalogConversion_s &raw_therm_fr) + { - therm_fl = front_thermistors.get(MCU15_THERM_FL_CHANNEL).convert(raw_therm_fl.raw); - therm_fr = front_thermistors.get(MCU15_THERM_FR_CHANNEL).convert(raw_therm_fr.raw); + auto fl_channel = _front_thermistors.get(MCU15_THERM_FL_CHANNEL); + auto fr_channel = _front_thermistors.get(MCU15_THERM_FR_CHANNEL); + + therm_fl = fl_channel.convert((uint16_t)raw_therm_fl.raw); + therm_fr = fr_channel.convert((uint16_t)raw_therm_fr.raw); update_front_thermistor_readings(); } \ No newline at end of file diff --git a/lib/mock_interfaces/AMSInterface.h b/lib/mock_interfaces/AMSInterface.h index a22d7db08..12a464304 100644 --- a/lib/mock_interfaces/AMSInterface.h +++ b/lib/mock_interfaces/AMSInterface.h @@ -6,6 +6,7 @@ #define CANBufferType_mock int #define CAN_message_t_mock int + /** * Mock interface for testing purposes. Please see the real AMSInterface for proper documentation. */ diff --git a/lib/mock_interfaces/AnalogSensorsInterface.h b/lib/mock_interfaces/AnalogSensorsInterface.h index 2fb8f1fdf..98d6d699e 100644 --- a/lib/mock_interfaces/AnalogSensorsInterface.h +++ b/lib/mock_interfaces/AnalogSensorsInterface.h @@ -4,24 +4,7 @@ #include #include -enum class AnalogSensorStatus_e -{ - ANALOG_SENSOR_GOOD = 0, - ANALOG_SENSOR_CLAMPED = 1, -}; - -struct AnalogConversion_s -{ - int raw; - float conversion; - AnalogSensorStatus_e status; -}; - -template -struct AnalogConversionPacket_s -{ - AnalogConversion_s conversions[N]; -}; +#include "SharedDataTypes.h" class AnalogChannel { diff --git a/lib/mock_interfaces/DashboardInterface.h b/lib/mock_interfaces/DashboardInterface.h index 67688fbec..57007a569 100644 --- a/lib/mock_interfaces/DashboardInterface.h +++ b/lib/mock_interfaces/DashboardInterface.h @@ -1,33 +1,7 @@ #ifndef DASHBOARDINTERFACE #define DASHBOARDINTERFACE -/* - Enum for the car's torque limits - MOVE ME! - ideally into a TorqueControllerDefs.h file - to prevent circular dependencies -*/ -enum class TorqueLimit_e -{ - TCMUX_LOW_TORQUE = 0, - TCMUX_MID_TORQUE = 1, - TCMUX_FULL_TORQUE = 2, - TCMUX_NUM_TORQUE_LIMITS = 3, -}; - -/* Enum for the modes on the dial, corresponds directly to dial index pos. */ -enum class DialMode_e -{ - /* No torque vectoring */ - MODE_0, - /* Normal force torque vectoring */ - MODE_1, - /* PID torque vectoring */ - MODE_2, - /* Launch Ctrl */ - MODE_3, - MODE_4, - MODE_5, -}; +#include "SharedDataTypes.h" /* Enum for defined LED colors. ON will be LED's default color on dashboard*/ enum class LEDColors_e @@ -61,6 +35,8 @@ class DashboardInterface public: bool buzzer = false; bool start_button_status_; + ControllerMode_e getDialMode() {return {};} + TorqueLimit_e getTorqueLimitMode() {return {};} bool startButtonPressed() { return start_button_status_; }; bool checkBuzzer(){ return buzzer; }; }; diff --git a/lib/mock_interfaces/HytechCANInterface.h b/lib/mock_interfaces/HytechCANInterface.h index e69de29bb..2628c9f49 100644 --- a/lib/mock_interfaces/HytechCANInterface.h +++ b/lib/mock_interfaces/HytechCANInterface.h @@ -0,0 +1,9 @@ +#ifndef __HYTECHCANINTERFACE_H__ +#define __HYTECHCANINTERFACE_H__ +#include "HT08_CASE_types.h" + +template +void enqueue_matlab_msg(bufferType *msg_queue, const CAN_MESSAGE_BUS & structure) +{ +} +#endif // __HYTECHCANINTERFACE_H__ \ No newline at end of file diff --git a/lib/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/lib/mock_interfaces/VectornavInterface.h b/lib/mock_interfaces/VectornavInterface.h index 05c486d6e..c686e3ceb 100644 --- a/lib/mock_interfaces/VectornavInterface.h +++ b/lib/mock_interfaces/VectornavInterface.h @@ -1,24 +1,7 @@ #ifndef VECTORNAVINTERFACE #define VECTORNAVINTERFACE +#include "SharedDataTypes.h" -struct vector_nav { - float velocity_x; - float velocity_y; - float velocity_z; - float linear_accel_x; - float linear_accel_y; - float linear_accel_z; - float uncompLinear_accel[3]; // 3D uncompensated linear acceleration - float yaw; - float pitch; - float roll; - double latitude; - double longitude; - double ecef_coords[3]; // x,y,z - uint64_t gps_time; // gps time - uint8_t vn_status; // status - xyz_vec angular_rates; - }; template class VNInterface @@ -27,7 +10,7 @@ class VNInterface /* Watchdog last kicked time */ message_queue *msg_queue_; uint32_t can_id_; - vector_nav vn_data; + VectornavData_s vn_data; public: VNInterface(message_queue *msg_output_queue) { @@ -36,7 +19,7 @@ class VNInterface /* Kick watchdog */ // getters - vector_nav get_vn_struct(); + VectornavData_s get_vn_struct(); uint32_t get_id() { return can_id_;}; }; diff --git a/lib/shared_data/include/PhysicalParameters.h b/lib/shared_data/include/PhysicalParameters.h new file mode 100644 index 000000000..c99f50bf7 --- /dev/null +++ b/lib/shared_data/include/PhysicalParameters.h @@ -0,0 +1,20 @@ +#ifndef __PHYSICALPARAMETERS_H__ +#define __PHYSICALPARAMETERS_H__ + +namespace PhysicalParameters +{ + const float AMK_MAX_RPM = 20000; + const float AMK_MAX_TORQUE = 21.42; + const float MAX_REGEN_TORQUE = 10.0; + +} +constexpr const float GEARBOX_RATIO = 11.86; +constexpr const float WHEEL_DIAMETER = 0.4064; // meters +constexpr const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; +constexpr const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0 / 1000.0; +constexpr const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; + +const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415 / 60.0; +const float RAD_PER_SECOND_TO_RPM = 1 / RPM_TO_RAD_PER_SECOND; + +#endif /* __PHYSICALPARAMETERS_H__ */ \ No newline at end of file diff --git a/lib/shared_data/include/SharedDataTypes.h b/lib/shared_data/include/SharedDataTypes.h new file mode 100644 index 000000000..2e399a6df --- /dev/null +++ b/lib/shared_data/include/SharedDataTypes.h @@ -0,0 +1,155 @@ +#ifndef TORQUECONTROLLERSDATA +#define TORQUECONTROLLERSDATA +#include +#include "Utility.h" +#include "SysClock.h" +#include "SharedFirmwareTypes.h" + +enum class TorqueLimit_e +{ + TCMUX_FULL_TORQUE = 0, + TCMUX_MID_TORQUE = 1, + TCMUX_LOW_TORQUE = 2, + TCMUX_NUM_TORQUE_LIMITS = 3, +}; + +/* Enum for the modes on the dial, corresponds directly to dial index pos. */ +enum class ControllerMode_e +{ + /* No torque vectoring */ + MODE_0, + /* Normal force torque vectoring */ + MODE_1, + /* PID torque vectoring */ + MODE_2, + /* Launch Ctrl */ + MODE_3, + MODE_4, + MODE_5, +}; + +struct PedalsSystemData_s +{ + bool accelImplausible : 1; + bool brakeImplausible : 1; + bool brakePressed : 1; + bool accelPressed : 1; + bool mechBrakeActive : 1; + bool brakeAndAccelPressedImplausibility : 1; + bool implausibilityExceededMaxDuration : 1; + float accelPercent; + float brakePercent; + float regenPercent; +}; +struct DrivetrainDynamicReport_s +{ + uint16_t measuredInverterFLPackVoltage; + float measuredSpeeds[NUM_MOTORS]; + float measuredTorques[NUM_MOTORS]; + float measuredTorqueCurrents[NUM_MOTORS]; + float measuredMagnetizingCurrents[NUM_MOTORS]; +}; +struct DrivetrainCommand_s +{ + float speeds_rpm[NUM_MOTORS]; + float torqueSetpoints[NUM_MOTORS]; // FIXME: misnomer. This represents the magnitude of the torque the inverter can command to reach the commanded speed setpoint +}; + +struct TorqueControllerOutput_s +{ + DrivetrainCommand_s command; + bool ready; +}; +struct VectornavData_s +{ + float velocity_x; + float velocity_y; + float velocity_z; + float linear_accel_x; + float linear_accel_y; + float linear_accel_z; + float uncompLinear_accel[3]; // 3D uncompensated linear acceleration + float yaw; + float pitch; + float roll; + double latitude; + double longitude; + double ecef_coords[3]; // x,y,z + uint64_t gps_time; // gps time + uint8_t vn_status; // status + xyz_vec angular_rates; +}; + +enum class TorqueControllerMuxError +{ + NO_ERROR = 0, + ERROR_SPEED_DIFF_TOO_HIGH = 1, + ERROR_TORQUE_DIFF_TOO_HIGH = 2, + ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS =3, + ERROR_CONTROLLER_NULL_POINTER =4 +}; + +struct TorqueControllerMuxStatus +{ + TorqueControllerMuxError current_error; + ControllerMode_e current_controller_mode; + TorqueLimit_e current_torque_limit_enum; + float current_torque_limit_value; + bool output_is_bypassing_limits; +}; + +struct LoadCellInterfaceOutput_s +{ + veh_vec loadCellForcesFiltered; + veh_vec loadCellConversions; + bool FIRSaturated; +}; + +// Enums +enum class SteeringSystemStatus_e +{ + STEERING_SYSTEM_NOMINAL = 0, + STEERING_SYSTEM_MARGINAL = 1, + STEERING_SYSTEM_DEGRADED = 2, + STEERING_SYSTEM_ERROR = 3, +}; + +struct SteeringSystemData_s +{ + float angle; + SteeringSystemStatus_e status; +}; + +/// @brief car state struct that contains state of everything about the car including +// things such as steering, drivetrain, current system time, vectornav / INS data, +// etc. an instance of this struct is created in main and updated there by all of the systems +// and interfaces and then shared between all of the systems and interfaces that need +// access to the state of the car. +struct SharedCarState_s +{ + // data + SysTick_s systick; + SteeringSystemData_s steering_data; + DrivetrainDynamicReport_s drivetrain_data; + LoadCellInterfaceOutput_s loadcell_data; + PedalsSystemData_s pedals_data; + VectornavData_s vn_data; + SharedCarState_s() = delete; + SharedCarState_s(const SysTick_s &_systick, + const SteeringSystemData_s &_steering_data, + const DrivetrainDynamicReport_s &_drivetrain_data, + const LoadCellInterfaceOutput_s &_loadcell_data, + const PedalsSystemData_s &_pedals_data, + const VectornavData_s &_vn_data) + : systick(_systick), + steering_data(_steering_data), + drivetrain_data(_drivetrain_data), + loadcell_data(_loadcell_data), + pedals_data(_pedals_data), + vn_data(_vn_data) + { + // constructor body (if needed) + } +}; + +#endif \ No newline at end of file diff --git a/lib/shared_data/include/TorqueControllersData.h b/lib/shared_data/include/TorqueControllersData.h deleted file mode 100644 index c35a0cb51..000000000 --- a/lib/shared_data/include/TorqueControllersData.h +++ /dev/null @@ -1,31 +0,0 @@ -#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/systems/include/Utility.h b/lib/shared_data/include/Utility.h similarity index 95% rename from lib/systems/include/Utility.h rename to lib/shared_data/include/Utility.h index dc2a9d508..820d0f4a4 100644 --- a/lib/systems/include/Utility.h +++ b/lib/shared_data/include/Utility.h @@ -1,6 +1,6 @@ #ifndef __UTILITY_H__ #define __UTILITY_H__ - +#include "PhysicalParameters.h" // Defines const int FL = 0; const int FR = 1; diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index 7668ad8cf..d142d4bdb 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -11,8 +11,7 @@ #include "SafetySystem.h" #include "DashboardInterface.h" #include "AMSInterface.h" - -// #include "IMDInterface.h" +#include "PrintLogger.h" enum class CAR_STATE { @@ -32,7 +31,7 @@ class MCUStateMachine DrivetrainSysType *drivetrain, DashboardInterface *dashboard, PedalsSystem *pedals, - TorqueControllerMux *mux, + TCMuxType *mux, SafetySystem *safety_system) { current_state_ = CAR_STATE::STARTUP; @@ -44,10 +43,12 @@ class MCUStateMachine safety_system_ = safety_system; } - /// @brief our components can use this time to tell when to do things. We can set this ourselves for testing purposes instead of using metro timers - /// @param current_millis the current millis() call - // void tick_state_machine(const SysTick_s &tick); - void tick_state_machine(unsigned long cm); + // TODO update this to just use the car_state struct instead of having to pass in cm + /// @brief function to tick the state machine. + /// @param cm current millis from systick + /// @param current_car_state current state of the car (not to be confused with the CAR_STATE enum which is the state machine state) + void tick_state_machine(unsigned long cm, const SharedCarState_s ¤t_car_state); + CAR_STATE get_state() { return current_state_; } bool car_in_ready_to_drive() { return current_state_ == CAR_STATE::READY_TO_DRIVE; }; @@ -72,7 +73,8 @@ class MCUStateMachine DashboardInterface *dashboard_; // IMDInterface *imd_; SafetySystem *safety_system_; - TorqueControllerMux *controller_mux_; + 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 6cbdcbf42..36be0afa0 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -1,7 +1,7 @@ #include "MCUStateMachine.h" template -void MCUStateMachine::tick_state_machine(unsigned long current_millis) +void MCUStateMachine::tick_state_machine(unsigned long current_millis, const SharedCarState_s ¤t_car_state) { switch (get_state()) { @@ -39,7 +39,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren // Serial.print(" "); // Serial.print(data.brakeAndAccelPressedImplausibility); // Serial.print(" "); - + // Serial.println("accel, brake:"); // Serial.print(data.accelPercent); // Serial.print(" "); @@ -48,7 +48,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren // Serial.print(data.implausibilityExceededMaxDuration); // Serial.println(); - // if TS is above HV threshold, move to Tractive System Active drivetrain_->disable_no_pins(); if (drivetrain_->hv_over_threshold_on_drivetrain()) @@ -128,7 +127,6 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::READY_TO_DRIVE: { auto data = pedals_->getPedalsSystemData(); - // auto test = controller_mux_->getDrivetrainCommand(); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { @@ -146,12 +144,13 @@ void MCUStateMachine::tick_state_machine(unsigned long curren if (safety_system_->get_software_is_ok() && !data.implausibilityExceededMaxDuration) { - drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand()); + 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 + else { drivetrain_->command_drivetrain_no_torque(); - } break; @@ -189,7 +188,7 @@ void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state case CAR_STATE::WAITING_READY_TO_DRIVE_SOUND: break; case CAR_STATE::READY_TO_DRIVE: - { + { // deactivate buzzer and reset it to turn on again later buzzer_->deactivate(); break; diff --git a/lib/systems/include/BaseController.h b/lib/systems/include/BaseController.h new file mode 100644 index 000000000..a94556fb9 --- /dev/null +++ b/lib/systems/include/BaseController.h @@ -0,0 +1,17 @@ +#ifndef BASECONTROLLER +#define BASECONTROLLER +#include "SharedDataTypes.h" +namespace BaseControllerParams +{ + const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { + .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, + .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}}; + +} +class Controller +{ +public: + virtual TorqueControllerOutput_s evaluate(const SharedCarState_s &state) = 0; +}; + +#endif \ No newline at end of file diff --git a/lib/systems/include/BaseLaunchController.h b/lib/systems/include/BaseLaunchController.h new file mode 100644 index 000000000..0a3fbd4a4 --- /dev/null +++ b/lib/systems/include/BaseLaunchController.h @@ -0,0 +1,55 @@ +#ifndef __BASELAUNCHCONTROLLER_H__ +#define __BASELAUNCHCONTROLLER_H__ +#include "SharedDataTypes.h" +#include "BaseController.h" +#include +#include +enum class LaunchStates_e +{ + NO_LAUNCH_MODE, + LAUNCH_NOT_READY, + LAUNCH_READY, + LAUNCHING +}; + +namespace BaseLaunchControllerParams +{ + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; + const float const_accel_time = 100; // time to use launch speed target in ms + const float launch_ready_accel_threshold = .1; + const float launch_ready_brake_threshold = .2; + const float launch_ready_speed_threshold = 5.0 * METERS_PER_SECOND_TO_RPM; // rpm + const float launch_go_accel_threshold = .9; + const float launch_stop_accel_threshold = .5; +} + +class BaseLaunchController : public virtual Controller +{ +protected: + TorqueControllerOutput_s writeout_; + uint32_t time_of_launch_; + double initial_ecef_x_; + double initial_ecef_y_; + double initial_ecef_z_; + LaunchStates_e launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; + uint32_t current_millis_; + float launch_speed_target_ = 0.0; + int16_t init_speed_target_ = 0; + +public: + BaseLaunchController(int16_t initial_speed_target) + : init_speed_target_(initial_speed_target) + { + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; + writeout_.ready = true; + } + + 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_; } + virtual void calc_launch_algo(const VectornavData_s &vn_data) = 0; + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; +}; +#endif // __BASELAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 9207da8d5..5e5c962f2 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -8,7 +8,7 @@ #include "SteeringSystem.h" #include "MCUStateMachine.h" #include "ProtobufMsgInterface.h" -#include "ParameterInterface.h" +// #include "ParameterInterface.h" struct CASEConfiguration { @@ -96,6 +96,7 @@ template class CASESystem { public: + CASESystem() = delete; /// @brief constructor for state estimator system. /// @param can_queue the pointer to the message queue that will have the CAN messages put onto it /// @param send_period_ms the period in which messages will be put into the queue to be sent in milliseconds. @@ -135,7 +136,7 @@ class CASESystem /// @return controller output DrivetrainCommand_s evaluate( const SysTick_s &tick, - const vector_nav &vn_data, + const VectornavData_s &vn_data, const SteeringSystemData_s &steering_data, const DrivetrainDynamicReport_s &drivetrain_data, const veh_vec &load_cell_vals, @@ -145,78 +146,12 @@ class CASESystem bool start_button_pressed, uint8_t vn_status); - // void update_pid(float yaw_p, float yaw_i, float yaw_d, float tcs_p, float tcs_i, float tcs_d, float brake_p, float brake_i, float brake_d) - // { - // config_.yaw_pid_p = yaw_p; - // config_.yaw_pid_p = yaw_i; - // config_.yaw_pid_p = yaw_d; + DrivetrainCommand_s get_current_drive_command() { return current_command_; } - // config_.tcs_pid_p = tcs_p; - // config_.tcs_pid_i = tcs_i; - // config_.tcs_pid_d = tcs_d; - - // config_.yaw_pid_brakes_p = brake_p; - // config_.yaw_pid_brakes_i = brake_i; - // config_.yaw_pid_brakes_d = brake_d; - // } - - float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm); + float calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_torque, float max_regen_torque, float max_rpm); /// @brief configuration function to determine what CASE is using / turn on and off different features within CASE /// @param config the configuration struct we will be setting - void update_config_from_param_interface(ParameterInterface ¶m_interface_ref) - { - config cfg = param_interface_ref.get_config(); - config_.AbsoluteTorqueLimit = cfg.AbsoluteTorqueLimit; - config_.yaw_pid_p = cfg.yaw_pid_p; - config_.yaw_pid_i = cfg.yaw_pid_i; - config_.yaw_pid_d = cfg.yaw_pid_d; - config_.tcs_pid_p_lowerBound_front = cfg.tcs_pid_p_lowerBound_front; - config_.tcs_pid_p_upperBound_front = cfg.tcs_pid_p_upperBound_front; - config_.tcs_pid_p_lowerBound_rear = cfg.tcs_pid_p_lowerBound_rear; - config_.tcs_pid_p_upperBound_rear = cfg.tcs_pid_p_upperBound_rear; - config_.tcs_pid_i = cfg.tcs_pid_i; - config_.tcs_pid_d = cfg.tcs_pid_d; - config_.useLaunch = cfg.useLaunch; - config_.usePIDTV = cfg.usePIDTV; - config_.useTCSLimitedYawPID = cfg.useTCSLimitedYawPID; - config_.useNormalForce = cfg.useNormalForce; - config_.useTractionControl = cfg.useTractionControl; - config_.usePowerLimit = cfg.usePowerLimit; - config_.usePIDPowerLimit = cfg.usePIDPowerLimit; - config_.useDecoupledYawBrakes = cfg.useDecoupledYawBrakes; - config_.useDiscontinuousYawPIDBrakes = cfg.useDiscontinuousYawPIDBrakes; - config_.tcsSLThreshold = cfg.tcsSLThreshold; - config_.launchSL = cfg.launchSL; - config_.launchDeadZone = cfg.launchDeadZone; - config_.launchVelThreshold = cfg.launchVelThreshold; - config_.tcsVelThreshold = cfg.tcsVelThreshold; - config_.yawPIDMaxDifferential = cfg.yawPIDMaxDifferential; - config_.yawPIDErrorThreshold = cfg.yawPIDErrorThreshold; - config_.yawPIDVelThreshold = cfg.yawPIDVelThreshold; - config_.yawPIDCoastThreshold = cfg.yawPIDCoastThreshold; - config_.yaw_pid_brakes_p = cfg.yaw_pid_brakes_p; - config_.yaw_pid_brakes_i = cfg.yaw_pid_brakes_i; - config_.yaw_pid_brakes_d = cfg.yaw_pid_brakes_d; - config_.decoupledYawPIDBrakesMaxDIfference = cfg.decoupledYawPIDBrakesMaxDIfference; - config_.discontinuousBrakesPercentThreshold = cfg.discontinuousBrakesPercentThreshold; - config_.TorqueMode = cfg.TorqueMode; - config_.RegenLimit = cfg.RegenLimit; - config_.useNoRegen5kph = cfg.useNoRegen5kph; - config_.useTorqueBias = cfg.useTorqueBias; - config_.DriveTorquePercentFront = cfg.DriveTorquePercentFront; - config_.BrakeTorquePercentFront = cfg.BrakeTorquePercentFront; - config_.MechPowerMaxkW = cfg.MechPowerMaxkW; - config_.launchLeftRightMaxDiff = cfg.launchLeftRightMaxDiff; - config_.tcs_pid_lower_rpm_front = cfg.tcs_pid_lower_rpm_front; - config_.tcs_pid_upper_rpm_front = cfg.tcs_pid_upper_rpm_front; - config_.tcs_pid_lower_rpm_rear = cfg.tcs_pid_lower_rpm_rear; - config_.tcs_pid_upper_rpm_rear = cfg.tcs_pid_upper_rpm_rear; - config_.maxNormalLoadBrakeScalingFront = cfg.maxNormalLoadBrakeScalingFront; - config_.max_rpm = cfg.max_rpm; - config_.max_regen_torque = cfg.max_regen_torque; - config_.max_torque = cfg.max_torque; - } float get_rpm_setpoint(float final_torque) { if (final_torque > 0) @@ -230,6 +165,7 @@ class CASESystem } private: + DrivetrainCommand_s current_command_; CASEConfiguration config_; message_queue *msg_queue_; HT08_CASE case_; diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 3a6db48c7..1787cbb12 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -1,9 +1,9 @@ #include "CASESystem.h" - +#include "SharedFirmwareTypes.h" template DrivetrainCommand_s CASESystem::evaluate( const SysTick_s &tick, - const vector_nav &vn_data, + const VectornavData_s &vn_data, const SteeringSystemData_s &steering_data, const DrivetrainDynamicReport_s &drivetrain_data, const veh_vec &load_cell_vals, @@ -18,7 +18,7 @@ DrivetrainCommand_s CASESystem::evaluate( // in. as defined in HT08_CASE.h, ExtU_HT08_CASE_T in.SteeringWheelAngleDeg = steering_data.angle; - in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_regen_torque, config_.max_rpm); + in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_torque, config_.max_regen_torque, config_.max_rpm); in.YawRaterads = vn_data.angular_rates.z; @@ -278,11 +278,12 @@ DrivetrainCommand_s CASESystem::evaluate( command.speeds_rpm[2] = get_rpm_setpoint(res.FinalTorqueRL); command.speeds_rpm[3] = get_rpm_setpoint(res.FinalTorqueRR); + current_command_ = command; return command; } template -float CASESystem::calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_regen_torque, float max_rpm) +float CASESystem::calculate_torque_request(const PedalsSystemData_s &pedals_data, float max_torque, float max_regen_torque, float max_rpm) { // accelRequest goes between 1.0 and -1.0 float accelRequest = pedals_data.accelPercent - pedals_data.regenPercent; @@ -291,7 +292,7 @@ float CASESystem::calculate_torque_request(const PedalsSystemData if (accelRequest >= 0.0) { // Positive torque request - torqueRequest = accelRequest * AMK_MAX_TORQUE; + torqueRequest = accelRequest * max_regen_torque; } else { diff --git a/lib/systems/include/Controllers/CASEControllerWrapper.h b/lib/systems/include/Controllers/CASEControllerWrapper.h new file mode 100644 index 000000000..8e0dbd7e8 --- /dev/null +++ b/lib/systems/include/Controllers/CASEControllerWrapper.h @@ -0,0 +1,30 @@ +#ifndef __CASECONTROLLERWRAPPER_H__ +#define __CASECONTROLLERWRAPPER_H__ +#include "BaseController.h" + +#include "CASESystem.h" + +template +class TorqueControllerCASEWrapper : public virtual Controller +{ +public: + TorqueControllerCASEWrapper() = delete; + + TorqueControllerCASEWrapper(CASESystem *case_instance) : case_instance_(case_instance) + { + } + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override + { + DrivetrainCommand_s curr_cmd = case_instance_->get_current_drive_command(); + TorqueControllerOutput_s out; + out.ready = true; + out.command = curr_cmd; + return out; + } + +private: + CASESystem *case_instance_; + +}; + +#endif // __CASECONTROLLERWRAPPER_H__ \ No newline at end of file diff --git a/lib/systems/include/Controllers/LoadCellVectoringController.h b/lib/systems/include/Controllers/LoadCellVectoringController.h new file mode 100644 index 000000000..dc7aaa373 --- /dev/null +++ b/lib/systems/include/Controllers/LoadCellVectoringController.h @@ -0,0 +1,69 @@ +#ifndef LOADCELLVECTORINGCONTROLLER +#define LOADCELLVECTORINGCONTROLLER + +#include "BaseController.h" +class TorqueControllerLoadCellVectoring : public virtual Controller +{ +private: + float frontTorqueScale_ = 1.0; + float rearTorqueScale_ = 1.0; + float frontRegenTorqueScale_ = 1.0; + float rearRegenTorqueScale_ = 1.0; + + TorqueControllerOutput_s writeout_; + /* + FIR filter designed with + http://t-filter.appspot.com + + sampling frequency: 100 Hz + + * 0 Hz - 10 Hz + gain = 1 + desired ripple = 5 dB + actual ripple = 1.7659949026015025 dB + + * 40 Hz - 50 Hz + gain = 0 + desired attenuation = -40 dB + actual attenuation = -47.34009380570117 dB + */ + const static int numFIRTaps_ = 5; + float FIRTaps_[numFIRTaps_] = { + 0.07022690881526232, + 0.27638313122745306, + 0.408090001549378, + 0.27638313122745306, + 0.07022690881526232}; + int FIRCircBufferHead = 0; // index of the latest sample in the raw buffer + float loadCellForcesRaw_[4][numFIRTaps_] = {}; + float loadCellForcesFiltered_[4] = {}; + // Some checks that can disable the controller + const int errorCountThreshold_ = 25; + int loadCellsErrorCounter_[4] = {}; + bool FIRSaturated_ = false; + bool ready_ = false; + +public: + /// @brief load cell TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance + /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command + /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD + /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear + TorqueControllerLoadCellVectoring(float rearTorqueScale, float regenTorqueScale) + : frontTorqueScale_(2.0 - rearTorqueScale), + rearTorqueScale_(rearTorqueScale), + frontRegenTorqueScale_(2.0 - regenTorqueScale), + rearRegenTorqueScale_(regenTorqueScale) + { + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; + writeout_.ready = false; + } + TorqueControllerLoadCellVectoring() : TorqueControllerLoadCellVectoring(1.0, 1.0) {} + + void tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + const LoadCellInterfaceOutput_s &loadCellData); + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; +}; + +#endif \ No newline at end of file diff --git a/lib/systems/include/Controllers/LookupLaunchController.h b/lib/systems/include/Controllers/LookupLaunchController.h new file mode 100644 index 000000000..9072005fd --- /dev/null +++ b/lib/systems/include/Controllers/LookupLaunchController.h @@ -0,0 +1,32 @@ +#ifndef __LOOKUPLAUNCHCONTROLLER_H__ +#define __LOOKUPLAUNCHCONTROLLER_H__ +#include "BaseLaunchController.h" +#include "accel_lookup.h" + +namespace LookupLaunchControllerParams +{ + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET; +}; +class TorqueControllerLookupLaunch : public virtual BaseLaunchController +{ +private: + bool init_position = false; + +public: + /*! + Lookup Launch Controller + This launch controller is based off of a matlab and symlink generated lookup table. + This has been converted to a C array with some basic python code using the array index + as the input for the controller + @param initial_speed_target the initial speed commanded to the wheels + */ + TorqueControllerLookupLaunch(int16_t initial_speed_target) + : BaseLaunchController(initial_speed_target) {} + + TorqueControllerLookupLaunch() : TorqueControllerLookupLaunch(LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} + + void calc_launch_algo(const VectornavData_s &vn_data) override; +}; + + +#endif // __LOOKUPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/Controllers/SimpleController.h b/lib/systems/include/Controllers/SimpleController.h new file mode 100644 index 000000000..33f691fee --- /dev/null +++ b/lib/systems/include/Controllers/SimpleController.h @@ -0,0 +1,36 @@ +#ifndef SIMPLECONTROLLER +#define SIMPLECONTROLLER + +#include "BaseController.h" + + + +class TorqueControllerSimple : public Controller +{ +private: + TorqueControllerOutput_s writeout_; + float frontTorqueScale_ = 1.0; + float rearTorqueScale_ = 1.0; + float frontRegenTorqueScale_ = 1.0; + float rearRegenTorqueScale_ = 1.0; + +public: + /// @brief simple TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance + /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command + /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD + /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear + TorqueControllerSimple(float rearTorqueScale, float regenTorqueScale) + : frontTorqueScale_(2.0 - rearTorqueScale), + rearTorqueScale_(rearTorqueScale), + frontRegenTorqueScale_(2.0 - regenTorqueScale), + rearRegenTorqueScale_(regenTorqueScale) + { + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; + writeout_.ready = true; + } + + void tick(const PedalsSystemData_s &pedalsData); + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override; +}; + +#endif \ No newline at end of file diff --git a/lib/systems/include/Controllers/SimpleLaunchController.h b/lib/systems/include/Controllers/SimpleLaunchController.h new file mode 100644 index 000000000..ef4f84a64 --- /dev/null +++ b/lib/systems/include/Controllers/SimpleLaunchController.h @@ -0,0 +1,36 @@ +#ifndef __SIMPLELAUNCHCONTROLLER_H__ +#define __SIMPLELAUNCHCONTROLLER_H__ +#include "BaseLaunchController.h" + +/* LAUNCH CONSTANTS */ +namespace SimpleLaunchControllerParams +{ + const float DEFAULT_LAUNCH_RATE = 11.76; + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET; + +}; +namespace SLParams = SimpleLaunchControllerParams; + +class TorqueControllerSimpleLaunch : public virtual BaseLaunchController +{ +private: + float launch_rate_target_; + +public: + /*! + SIMPLE LAUNCH CONTROLLER + This launch controller is based off of a specified launch rate and an initial speed target + It will ramp up the speed target linearlly over time to accelerate + @param launch_rate specified launch rate in m/s^2 + @param initial_speed_target the initial speed commanded to the wheels + */ + TorqueControllerSimpleLaunch(float launch_rate, int16_t initial_speed_target) + : BaseLaunchController(initial_speed_target), + launch_rate_target_(launch_rate) {} + + + TorqueControllerSimpleLaunch() : TorqueControllerSimpleLaunch(SLParams::DEFAULT_LAUNCH_RATE, SLParams::DEFAULT_LAUNCH_SPEED_TARGET) {} + + 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 new file mode 100644 index 000000000..f3b61eeb6 --- /dev/null +++ b/lib/systems/include/Controllers/SlipLaunchController.h @@ -0,0 +1,32 @@ +#ifndef __SLIPLAUNCHCONTROLLER_H__ +#define __SLIPLAUNCHCONTROLLER_H__ +#include "SimpleLaunchController.h" + +namespace SlipLaunchControllerParams +{ + const int16_t DEFAULT_LAUNCH_SPEED_TARGET = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET; + const float DEFAULT_SLIP_RATIO = 0.2f; +}; +class TorqueControllerSlipLaunch : public virtual BaseLaunchController +{ +private: + float slip_ratio_; + +public: + /*! + SLIP LAUNCH CONTROLLER + This launch controller is based off of a specified slip constant. It will at all times attempt + to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it + in constant slip + @param slip_ratio specified launch rate in m/s^2 + @param initial_speed_target the initial speed commanded to the wheels + */ + TorqueControllerSlipLaunch(float slip_ratio, int16_t initial_speed_target) + : BaseLaunchController(initial_speed_target), + slip_ratio_(slip_ratio) {} + + TorqueControllerSlipLaunch() : TorqueControllerSlipLaunch(SlipLaunchControllerParams::DEFAULT_SLIP_RATIO, SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET) {} + + void calc_launch_algo(const VectornavData_s &vn_data) override; +}; +#endif // __SLIPLAUNCHCONTROLLER_H__ \ No newline at end of file diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index 7a2c05640..7afb108ca 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -6,21 +6,8 @@ #include "stdint.h" #include "SysClock.h" #include "MCUInterface.h" +#include "SharedDataTypes.h" -struct DrivetrainCommand_s -{ - float speeds_rpm[NUM_MOTORS]; - float torqueSetpoints[NUM_MOTORS]; // FIXME: misnomer. This represents the magnitude of the torque the inverter can command to reach the commanded speed setpoint -}; - -struct DrivetrainDynamicReport_s -{ - uint16_t measuredInverterFLPackVoltage; - float measuredSpeeds[NUM_MOTORS]; - float measuredTorques[NUM_MOTORS]; - float measuredTorqueCurrents[NUM_MOTORS]; - float measuredMagnetizingCurrents[NUM_MOTORS]; -}; template class DrivetrainSystem diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index a47acd821..6986f9a10 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -3,23 +3,23 @@ #include #include #include "AnalogSensorsInterface.h" - +#include "SharedDataTypes.h" #include "SysClock.h" /// @brief system interface struct that contains the data from the pedal system -struct PedalsSystemData_s -{ - bool accelImplausible : 1; - bool brakeImplausible : 1; - bool brakePressed : 1; - bool accelPressed : 1; - bool mechBrakeActive : 1; - bool brakeAndAccelPressedImplausibility : 1; - bool implausibilityExceededMaxDuration : 1; - float accelPercent; - float brakePercent; - float regenPercent; -}; +// struct PedalsSystemData_s +// { +// bool accelImplausible : 1; +// bool brakeImplausible : 1; +// bool brakePressed : 1; +// bool accelPressed : 1; +// bool mechBrakeActive : 1; +// bool brakeAndAccelPressedImplausibility : 1; +// bool implausibilityExceededMaxDuration : 1; +// float accelPercent; +// float brakePercent; +// float regenPercent; +// }; /// @brief Pedals params struct that will hold min / max that will be used for evaluateion. struct PedalsParams @@ -203,4 +203,4 @@ class PedalsSystem }; -#endif /* PEDALSSYSTEM */ +#endif /* PEDALSSYSTEM */ \ No newline at end of file diff --git a/lib/systems/include/PhysicalParameters.h b/lib/systems/include/PhysicalParameters.h deleted file mode 100644 index 7f971575a..000000000 --- a/lib/systems/include/PhysicalParameters.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef __PHYSICALPARAMETERS_H__ -#define __PHYSICALPARAMETERS_H__ - -const float GEARBOX_RATIO = 11.86; -const float WHEEL_DIAMETER = 0.4064; // meters -const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; -const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0 / 1000.0; -const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; - -const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415 / 60.0; -const float RAD_PER_SECOND_TO_RPM = 1 / RPM_TO_RAD_PER_SECOND; - -#endif /* __PHYSICALPARAMETERS_H__ */ \ No newline at end of file diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index 3cec2e4a9..b082ace50 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -5,8 +5,8 @@ #include "AnalogSensorsInterface.h" #include "Filter_IIR.h" #include "SysClock.h" +#include "SharedDataTypes.h" #include "TelemetryInterface.h" - // Digital Encoder = Primary Sensor // Analog Encoder = Secondary Sensor @@ -17,14 +17,7 @@ #define NUM_SENSORS 2 #define DEFAULT_STEERING_ALPHA (0.0) -// Enums -enum class SteeringSystemStatus_e -{ - STEERING_SYSTEM_NOMINAL = 0, - STEERING_SYSTEM_MARGINAL = 1, - STEERING_SYSTEM_DEGRADED = 2, - STEERING_SYSTEM_ERROR = 3, -}; + // Structs struct SteeringSystemTick_s @@ -33,11 +26,6 @@ struct SteeringSystemTick_s const AnalogConversion_s &secondaryConversion; }; -struct SteeringSystemData_s -{ - float angle; - SteeringSystemStatus_e status; -}; class SteeringSystem { diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index f56731ba1..1db0cfb80 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -1,157 +1,126 @@ -#ifndef __TORQUECTRLMUX_H__ -#define __TORQUECTRLMUX_H__ - -#include -#include - -#include "TorqueControllers.h" -#include "DrivetrainSystem.h" -#include "PedalsSystem.h" -#include "SteeringSystem.h" -#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 +#ifndef TORQUECONTROLLERMUX +#define TORQUECONTROLLERMUX + +#include +#include "SharedDataTypes.h" +#include "BaseController.h" + +// notes: +// 21 torque limit should be first + +// tc mux needs to handle these things: +// 1 swapping between controller outputs +// 2 turning on and off running of controllers +// 3 application of safeties and limits to controller outputs +// 4 torque limit changing (torque mode) --> +// TODO the torque limit value changing should be handled in the dashboard interface + + +// TODOs +// - [x] make the controllers inherit from the base controller class +// - [x] port TorqueControllerSimple +// - [x] port TorqueControllerLoadCellVectoring +// - [x] port BaseLaunchController +// - [x] port TorqueControllerSimpleLaunch +// - [x] port slip launch +// - [x] port TorqueControllerLookupLaunch +// - [x] port CASE +// - [x] add the torque limit evaluation logic into dashboard interface +// - [x] integrate into state machine +// - [x] pass through the car state +// - [x] get dial and torque mode from the dashboard +// - [x] create car_state in main and pass into state machine +// - [x] add 3 bit status to a telemetry message for the TC mux status to HT_CAN +// - [x] pass state of tc mux into telem interface and add the CAN signal +// - [x] remove the old tc mux +// - [ ] add back checking of the ready flag of the controllers and if the controller isnt ready it defaults +// - [ ] add test for this +// - [ ] make folder for the controllers +// - [ ] write integration tests for the real controllers +// - [x] test construction with real controllers +// - [ ] ensure that sane outputs occur on first tick of each controller +// - [x] update the state machine unit test with integration test of new tc mux + +// ON CAR testing +// - [ ] test the change of the torque mode from the dashboard interface +// - [ ] write testing code for this in separate environment + + +namespace TC_MUX_DEFAULT_PARAMS +{ + 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; +}; -/// @brief multiplexer class for managing available torque controllers +template class TorqueControllerMux { -private: - TorqueControllerNone torqueControllerNone_; - TorqueControllerSimple torqueControllerSimple_; - TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; - TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_; - TorqueControllerSlipLaunch torqueControllerSlipLaunch_; - TorqueControllerCASEWrapper tcCASEWrapper_; - - // Use this to map the dial to TCMUX modes - std::unordered_map dialModeMap_ = { - {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, - {DialMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING}, - {DialMode_e::MODE_2, TorqueController_e::TC_CASE_SYSTEM}, - {DialMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH}, - {DialMode_e::MODE_4, TorqueController_e::TC_SLIP_LAUNCH}, - {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, - }; - std::unordered_map torqueLimitMap_ = { - {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0}, - {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, - {TorqueLimit_e::TCMUX_FULL_TORQUE, AMK_MAX_TORQUE} - }; - - TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; - DialMode_e currDialMode_ = DialMode_e::MODE_0; - - TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; + static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); + - // Handle array for all torque controllers - TorqueControllerBase* controllers[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)] = { - static_cast(&torqueControllerNone_), - static_cast(&torqueControllerSimple_), - static_cast(&torqueControllerLoadCellVectoring_), - static_cast(&torqueControllerSimpleLaunch_), - static_cast(&torqueControllerSlipLaunch_), - static_cast(&tcCASEWrapper_) - }; - - // Status tracking structure for visibility - TCMuxStatus_s tcMuxStatus_; - - DrivetrainCommand_s drivetrainCommand_; - TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_FULL_TORQUE; - bool torqueLimitButtonPressed_ = false; - unsigned long torqueLimitButtonPressedTime_ = 0; - TelemetryInterface *telemHandle_; +private: + std::array controller_pointers_; + + std::array mux_bypass_limits_; + + std::unordered_map torque_limit_map_ = { + {TorqueLimit_e::TCMUX_FULL_TORQUE, PhysicalParameters::AMK_MAX_TORQUE}, + {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0f}, + {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0f}}; + float max_change_speed_, max_torque_pos_change_delta_, max_power_limit_; + DrivetrainCommand_s prev_command_ = {}; + TorqueControllerMuxStatus current_status_ = {}; + TorqueControllerMuxError can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + DrivetrainCommand_s previous_controller_command, + DrivetrainCommand_s desired_controller_out); + + DrivetrainCommand_s apply_positive_speed_limit_(const DrivetrainCommand_s &command); + DrivetrainCommand_s apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque); + DrivetrainCommand_s apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque); + DrivetrainCommand_s apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data); + public: - /// @brief torque controller mux in which default instances of all torque controllers are created for use - TorqueControllerMux(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)]) - , 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, 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)]) - , telemHandle_(telemInterface) {} - - // Functions - /// @brief tick controllers to calculate drivetrain command - void tick( - const SysTick_s &tick, - const DrivetrainDynamicReport_s &drivetrainData, - const PedalsSystemData_s &pedalsData, - 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() - { - return torqueLimit_; - }; + TorqueControllerMux() = delete; + /// @brief constructor for the TC mux + /// @param controller_pointers the array of pointers to the controllers being muxed between + /// @param mux_bypass_limits the array of aligned bools for determining if the limits should be applied to the controller outputs defaults to TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + /// @param max_change_speed the max speed difference between the requested controller output and the actual speed of each wheel that if the controller has a diff larger than the mux will not switch to the requested controller + /// @param max_torque_pos_change_delta same as speed but evaluated between the controller commanded torques defaults to TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE + /// @param max_power_limit the max power limit defaults to TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT + explicit TorqueControllerMux(std::array controller_pointers, + std::array mux_bypass_limits, + float max_change_speed = TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE, + float max_torque_pos_change_delta = TC_MUX_DEFAULT_PARAMS::MAX_TORQUE_DELTA_FOR_MODE_CHANGE, + float max_power_limit = TC_MUX_DEFAULT_PARAMS::MAX_POWER_LIMIT) : controller_pointers_(controller_pointers), + mux_bypass_limits_(mux_bypass_limits), + max_change_speed_(max_change_speed), + max_torque_pos_change_delta_(max_torque_pos_change_delta), + max_power_limit_(max_power_limit) - const float getMaxTorque() - { - return torqueLimitMap_[torqueLimit_]; - } - const DialMode_e getDialMode() { - return currDialMode_; - } + static_assert(num_controllers > 0, "Must create TC mux with at least 1 controller"); - const TorqueController_e getDriveMode() - { - return muxMode_; } - - TorqueControllerBase* activeController() - { - // check to make sure that there is actually a controller - // at the muxMode_ idx - if (controllers[muxMode_] != NULL) { - return controllers[muxMode_]; - } else { - return static_cast(&torqueControllerNone_); - } - } - - /// @brief report TCMux status through Telemetry via CAN - void reportTCMuxStatus(); + const TorqueControllerMuxStatus &get_tc_mux_status() { return current_status_; } + + /// @brief function that evaluates the mux, controllers and gets the current command + /// @param requested_controller_type the requested controller type from the dial state + /// @param controller_command_torque_limit the torque limit state enum set by dashboard + /// @param input_state the current state of the car + /// @return the current drivetrain command to be sent to the drivetrain + DrivetrainCommand_s getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e controller_command_torque_limit, + const SharedCarState_s &input_state); }; +// } + +const int number_of_controllers = 5; +using TCMuxType = TorqueControllerMux; -#endif /* __TORQUECTRLMUX_H__ */ +#include "TorqueControllerMux.tpp" +#endif // __TorqueControllerMux_H__ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.tpp b/lib/systems/include/TorqueControllerMux.tpp new file mode 100644 index 000000000..c392240bd --- /dev/null +++ b/lib/systems/include/TorqueControllerMux.tpp @@ -0,0 +1,210 @@ +#include "TorqueControllerMux.h" + +template +DrivetrainCommand_s TorqueControllerMux::getDrivetrainCommand(ControllerMode_e requested_controller_type, + TorqueLimit_e requested_torque_limit, + const SharedCarState_s &input_state) +{ + + DrivetrainCommand_s empty_command = BaseControllerParams::TC_COMMAND_NO_TORQUE; + + TorqueControllerOutput_s current_output = { + .command = empty_command, + .ready = true}; + + int req_controller_mode_index = static_cast(requested_controller_type); + int current_controller_mode_index = static_cast(current_status_.current_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; + return empty_command; + } + + if( (!(controller_pointers_[current_controller_mode_index])) || (!controller_pointers_[req_controller_mode_index])) + { + current_status_.current_error = TorqueControllerMuxError::ERROR_CONTROLLER_NULL_POINTER; + return empty_command; + } + + current_output = controller_pointers_[current_controller_mode_index]->evaluate(input_state); + + // std::cout << "output torques " << current_output.command.torqueSetpoints[0] << " " << current_output.command.torqueSetpoints[1] << " " << current_output.command.torqueSetpoints[2] << " " << current_output.command.torqueSetpoints[3] << std::endl; + bool requesting_controller_change = requested_controller_type != current_status_.current_controller_mode; + + if (requesting_controller_change) + { + TorqueControllerOutput_s proposed_output = controller_pointers_[req_controller_mode_index]->evaluate(input_state); + TorqueControllerMuxError error_state = can_switch_controller_(input_state.drivetrain_data, current_output.command, proposed_output.command); + if (error_state == TorqueControllerMuxError::NO_ERROR) + { + current_status_.current_controller_mode = requested_controller_type; + current_controller_mode_index = req_controller_mode_index; + current_output = proposed_output; + } + current_status_.current_error = error_state; + } + if (!mux_bypass_limits_[current_controller_mode_index]) + { + current_status_.current_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]; + 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; + return current_output.command; +} + +template +TorqueControllerMuxError TorqueControllerMux::can_switch_controller_(DrivetrainDynamicReport_s current_drivetrain_data, + DrivetrainCommand_s previous_controller_command, + DrivetrainCommand_s desired_controller_out) +{ + bool speedPreventsModeChange = false; + // Check if torque delta permits mode change + bool torqueDeltaPreventsModeChange = false; + for (int i = 0; i < NUM_MOTORS; i++) + { + speedPreventsModeChange = (abs(current_drivetrain_data.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND) >= max_change_speed_); + // only if the torque delta is positive do we not want to switch to the new one + torqueDeltaPreventsModeChange = (desired_controller_out.torqueSetpoints[i] - previous_controller_command.torqueSetpoints[i]) > max_torque_pos_change_delta_; + if (speedPreventsModeChange) + { + return TorqueControllerMuxError::ERROR_SPEED_DIFF_TOO_HIGH; + } + if (torqueDeltaPreventsModeChange) + { + return TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH; + } + } + return TorqueControllerMuxError::NO_ERROR; +} + +/* Apply limit such that wheelspeed never goes negative */ +template +DrivetrainCommand_s TorqueControllerMux::apply_positive_speed_limit_(const DrivetrainCommand_s &command) +{ + DrivetrainCommand_s out; + out = command; + for (int i = 0; i < NUM_MOTORS; i++) + { + out.speeds_rpm[i] = std::max(0.0f, out.speeds_rpm[i]); + } + return out; +} + +template +DrivetrainCommand_s TorqueControllerMux::apply_torque_limit_(const DrivetrainCommand_s &command, float max_torque) +{ + DrivetrainCommand_s out = command; + float avg_torque = 0; + // get the average torque accross all 4 wheels + for (int i = 0; i < NUM_MOTORS; i++) + { + + avg_torque += abs(out.torqueSetpoints[i]); + } + avg_torque /= NUM_MOTORS; + + // if this is greather than the torque limit, scale down + if (avg_torque > max_torque) + { + // get the scale of avg torque above max torque + float scale = avg_torque / max_torque; + // divide by scale to lower avg below max torque + for (int i = 0; i < NUM_MOTORS; i++) + { + out.torqueSetpoints[i] = out.torqueSetpoints[i] / scale; + } + } + return out; +} + +/* + Apply power limit such that the mechanical power of all wheels never + exceeds the preset mechanical power limit. Scales all wheels down to + preserve functionality of torque controllers +*/ +template +DrivetrainCommand_s TorqueControllerMux::apply_power_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain, float power_limit, float max_torque) +{ + DrivetrainCommand_s out = command; + float net_torque_mag = 0; + float net_power = 0; + + // calculate current mechanical power + for (int i = 0; i < NUM_MOTORS; i++) + { + // get the total magnitude of torque from all 4 wheels + // sum up net torque + net_torque_mag += abs(out.torqueSetpoints[i]); + // calculate P = T*w for each wheel and sum together + net_power += abs(out.torqueSetpoints[i] * (drivetrain.measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + } + + // only evaluate power limit if current power exceeds it + if (net_power > power_limit) + { + // std::cout <<"net power too big" < +DrivetrainCommand_s TorqueControllerMux::apply_regen_limit_(const DrivetrainCommand_s &command, const DrivetrainDynamicReport_s &drivetrain_data) +{ + DrivetrainCommand_s out = command; + const float noRegenLimitKPH = 10.0; + const float fullRegenLimitKPH = 5.0; + float maxWheelSpeed = 0.0; + float torqueScaleDown = 0.0; + bool allWheelsRegen = true; // true when all wheels are targeting speeds below the current wheel speed + + for (int i = 0; i < NUM_MOTORS; i++) + { + maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain_data.measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); + + allWheelsRegen &= (out.speeds_rpm[i] < abs(drivetrain_data.measuredSpeeds[i]) || out.speeds_rpm[i] == 0); + } + + // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH + // linearly interpolate the scale factor between noRegenLimitKPH and fullRegenLimitKPH + torqueScaleDown = std::min(1.0f, std::max(0.0f, (maxWheelSpeed - fullRegenLimitKPH) / (noRegenLimitKPH - fullRegenLimitKPH))); + + if (allWheelsRegen) + { + for (int i = 0; i < NUM_MOTORS; i++) + { + out.torqueSetpoints[i] *= torqueScaleDown; + } + } + return out; +} \ No newline at end of file diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 198d4c118..cbafecffa 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -1,331 +1,7 @@ -#ifndef __TORQUECONTROLLERS_H__ -#define __TORQUECONTROLLERS_H__ - -#include -#include -#include -#include "AnalogSensorsInterface.h" -#include "DashboardInterface.h" -#include "PhysicalParameters.h" -#include "VectornavInterface.h" -#include "SteeringSystem.h" -#include "LoadCellInterface.h" - -#include "accel_lookup.h" - -#include "TorqueControllersData.h" - - -/* CONTROLLER CONSTANTS */ - -const float MAX_POWER_LIMIT = 51500.0; // max mechanical power limit in KW - -/* MOTOR CONSTANTS */ - -const float AMK_MAX_RPM = 20000; -// 10MPH LIMIT for lot testing lmao -// const float AMK_MAX_RPM = (13.4 * METERS_PER_SECOND_TO_RPM); // 30mph -// const float AMK_MAX_RPM = (4.47 * METERS_PER_SECOND_TO_RPM); // 10mph -// const float AMK_MAX_RPM = (2.235 * METERS_PER_SECOND_TO_RPM); // 5mph -// const float AMK_MAX_RPM = (.89 * METERS_PER_SECOND_TO_RPM); // 1mph -// const float -const float AMK_MAX_TORQUE = 21.42; -const float MAX_REGEN_TORQUE = 10.0; - -/* LAUNCH CONSTANTS */ - -const float DEFAULT_LAUNCH_RATE = 1.5 * 9.8; -const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500; - -const float DEFAULT_SLIP_RATIO = 0.2f; -const float const_accel_time = 100; // time to use launch speed target in ms - -const float launch_ready_accel_threshold = .1; -const float launch_ready_brake_threshold = .2; -const float launch_ready_speed_threshold = 5.0 * METERS_PER_SECOND_TO_RPM; // rpm -const float launch_go_accel_threshold = .9; -const float launch_stop_accel_threshold = .5; - -/* DRIVETRAIN STRUCTS */ - -const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { - .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, - .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}}; - -struct TorqueControllerOutput_s -{ - DrivetrainCommand_s command; - bool ready; -}; - -/* TC STRUCTS */ -struct TCCaseWrapperTick_s -{ - const DrivetrainCommand_s &command; - const SteeringSystemData_s &steeringData; -}; - -/* ENUMS */ - -enum TorqueController_e -{ - TC_NO_CONTROLLER = 0, - TC_SAFE_MODE = 1, - TC_LOAD_CELL_VECTORING = 2, - TC_SIMPLE_LAUNCH = 3, - TC_SLIP_LAUNCH = 4, - TC_LOOKUP_LAUNCH = 5, - TC_CASE_SYSTEM = 6, - TC_NUM_CONTROLLERS = 7, -}; - -enum class LaunchStates_e -{ - NO_LAUNCH_MODE, - LAUNCH_NOT_READY, - LAUNCH_READY, - LAUNCHING -}; - -/* TORQUE CONTROLLERS */ - -/* - Base torque controller to allow access to internal torque controller members -*/ -class TorqueControllerBase -{ -public: - /* returns the launch state for the purpose of lighting the dahsboard LED and unit testing. To be overridden in launch torque modes */ - virtual LaunchStates_e get_launch_state() { return LaunchStates_e::NO_LAUNCH_MODE; } -}; - -template -class TorqueController : public TorqueControllerBase -{ -protected: - -public: -}; - -class TorqueControllerNone : public TorqueController -{ -private: -public: - TorqueControllerNone(TorqueControllerOutput_s &writeout) - { - writeout.command = TC_COMMAND_NO_TORQUE; - writeout.ready = true; - }; -}; - -/// @brief Simple torque controller that only considers pedal inputs and torque limit -class TorqueControllerSimple : public TorqueController -{ -private: - TorqueControllerOutput_s &writeout_; - float frontTorqueScale_ = 1.0; - float rearTorqueScale_ = 1.0; - float frontRegenTorqueScale_ = 1.0; - float rearRegenTorqueScale_ = 1.0; - -public: - /// @brief simple TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance - /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command - /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD - /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear - TorqueControllerSimple(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), - rearTorqueScale_(rearTorqueScale), - frontRegenTorqueScale_(regenTorqueScale), - rearRegenTorqueScale_(1.0 - regenTorqueScale) - { - writeout_.command = TC_COMMAND_NO_TORQUE; - writeout_.ready = true; - } - TorqueControllerSimple(TorqueControllerOutput_s &writeout) : TorqueControllerSimple(writeout, 1.0, 1.0) {} - - void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit); -}; - -class TorqueControllerLoadCellVectoring : public TorqueController -{ -private: - TorqueControllerOutput_s &writeout_; - float frontTorqueScale_ = 1.0; - float rearTorqueScale_ = 1.0; - float frontRegenTorqueScale_ = 1.0; - float rearRegenTorqueScale_ = 1.0; - /* - FIR filter designed with - http://t-filter.appspot.com - - sampling frequency: 100 Hz - - * 0 Hz - 10 Hz - gain = 1 - desired ripple = 5 dB - actual ripple = 1.7659949026015025 dB - - * 40 Hz - 50 Hz - gain = 0 - desired attenuation = -40 dB - actual attenuation = -47.34009380570117 dB - */ - const static int numFIRTaps_ = 5; - float FIRTaps_[numFIRTaps_] = { - 0.07022690881526232, - 0.27638313122745306, - 0.408090001549378, - 0.27638313122745306, - 0.07022690881526232}; - int FIRCircBufferHead = 0; // index of the latest sample in the raw buffer - float loadCellForcesRaw_[4][numFIRTaps_] = {}; - float loadCellForcesFiltered_[4] = {}; - // Some checks that can disable the controller - const int errorCountThreshold_ = 25; - int loadCellsErrorCounter_[4] = {}; - bool FIRSaturated_ = false; - bool ready_ = false; - -public: - /// @brief load cell TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance - /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command - /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD - /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear - TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), - rearTorqueScale_(rearTorqueScale), - frontRegenTorqueScale_(regenTorqueScale), - rearRegenTorqueScale_(1.0 - regenTorqueScale) - { - writeout_.command = TC_COMMAND_NO_TORQUE; - writeout_.ready = false; - } - TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0, 1.0) {} - - void tick( - const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - float torqueLimit, - const LoadCellInterfaceOutput_s &loadCellData - ); -}; - -class BaseLaunchController -{ -protected: - TorqueControllerOutput_s &writeout_; - uint32_t time_of_launch_; - double initial_ecef_x_; - double initial_ecef_y_; - double initial_ecef_z_; - LaunchStates_e launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; - uint32_t current_millis_; - float launch_speed_target_ = 0.0; - int16_t init_speed_target_ = 0.0; -public: - BaseLaunchController(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) - : writeout_(writeout), - init_speed_target_(initial_speed_target) - { - writeout.command = TC_COMMAND_NO_TORQUE; - writeout_.ready = true; - } - - void tick(const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - const float wheel_rpms[], - const vector_nav *vn_data); - - virtual void calc_launch_algo(const vector_nav *vn_data) = 0; -}; - -class TorqueControllerSimpleLaunch : public TorqueController, public BaseLaunchController -{ -private: - float launch_rate_target_; - -public: - /*! - SIMPLE LAUNCH CONTROLLER - This launch controller is based off of a specified launch rate and an initial speed target - It will ramp up the speed target linearlly over time to accelerate - @param launch_rate specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout, float launch_rate, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target), - launch_rate_target_(launch_rate) {} - - TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSimpleLaunch(writeout, DEFAULT_LAUNCH_RATE, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; - -class TorqueControllerSlipLaunch : public TorqueController, public BaseLaunchController -{ -private: - float slip_ratio_; - -public: - /*! - SLIP LAUNCH CONTROLLER - This launch controller is based off of a specified slip constant. It will at all times attempt - to keep the wheelspeed at this certain higher percent of the body velocity of the car to keep it - in constant slip - @param slip_ratio specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout, float slip_ratio, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target), - slip_ratio_(slip_ratio) {} - - TorqueControllerSlipLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSlipLaunch(writeout, DEFAULT_SLIP_RATIO, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; - -class TorqueControllerLookupLaunch : public TorqueController, BaseLaunchController -{ -private: - bool init_position = false; - -public: - /*! - Lookup Launch Controller - This launch controller is based off of a matlab and symlink generated lookup table. - This has been converted to a C array with some basic python code using the array index - as the input for the controller - @param slip_ratio specified launch rate in m/s^2 - @param initial_speed_target the initial speed commanded to the wheels - */ - TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout, int16_t initial_speed_target) - : BaseLaunchController(writeout, initial_speed_target) {} - - TorqueControllerLookupLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerLookupLaunch(writeout, DEFAULT_LAUNCH_SPEED_TARGET) {} - - LaunchStates_e get_launch_state() override { return launch_state_; } - - void calc_launch_algo(const vector_nav *vn_data) override; -}; - -class TorqueControllerCASEWrapper : public TorqueController -{ -public: - void tick(const TCCaseWrapperTick_s &intake); - TorqueControllerCASEWrapper(TorqueControllerOutput_s &writeout) : writeout_(writeout) - { - writeout_ = writeout; - } - -private: - TorqueControllerOutput_s &writeout_; -}; - -#endif /* __TORQUECONTROLLERS_H__ */ +//controllers +#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/Controllers/BaseLaunchController.cpp b/lib/systems/src/Controllers/BaseLaunchController.cpp new file mode 100644 index 000000000..c0bd646db --- /dev/null +++ b/lib/systems/src/Controllers/BaseLaunchController.cpp @@ -0,0 +1,114 @@ +#include "BaseLaunchController.h" +#include /* abs */ +void BaseLaunchController::tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + const float wheel_rpms[], + const VectornavData_s &vn_data) +{ + + if (tick.triggers.trigger100) + { + + current_millis_ = tick.millis; + + int16_t brake_torque_req = pedalsData.regenPercent * PhysicalParameters::MAX_REGEN_TORQUE; + + float max_speed = 0; + for (int i = 0; i < 4; i++) + { + max_speed = std::max(max_speed, abs(wheel_rpms[i])); + } + + writeout_.ready = true; + + switch (launch_state_) + { + case LaunchStates_e::LAUNCH_NOT_READY: + // set torques and speed to 0 + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + writeout_.command.speeds_rpm[RL] = 0.0; + writeout_.command.speeds_rpm[RR] = 0.0; + + writeout_.command.torqueSetpoints[FL] = brake_torque_req; + writeout_.command.torqueSetpoints[FR] = brake_torque_req; + writeout_.command.torqueSetpoints[RL] = brake_torque_req; + writeout_.command.torqueSetpoints[RR] = brake_torque_req; + + // init launch vars + launch_speed_target_ = 0; + time_of_launch_ = tick.millis; + // check speed is 0 and pedals not pressed + if ((pedalsData.accelPercent < BaseLaunchControllerParams::launch_ready_accel_threshold) && (pedalsData.brakePercent < BaseLaunchControllerParams::launch_ready_brake_threshold) && (max_speed < BaseLaunchControllerParams::launch_ready_speed_threshold)) + { + launch_state_ = LaunchStates_e::LAUNCH_READY; + } + + break; + case LaunchStates_e::LAUNCH_READY: + // set torques and speed to 0 + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + writeout_.command.speeds_rpm[RL] = 0.0; + writeout_.command.speeds_rpm[RR] = 0.0; + + writeout_.command.torqueSetpoints[FL] = brake_torque_req; + writeout_.command.torqueSetpoints[FR] = brake_torque_req; + writeout_.command.torqueSetpoints[RL] = brake_torque_req; + writeout_.command.torqueSetpoints[RR] = brake_torque_req; + + // init launch vars + launch_speed_target_ = 0; + time_of_launch_ = current_millis_; + + // check speed is 0 and brake not pressed + if ((pedalsData.brakePercent >= BaseLaunchControllerParams::launch_ready_brake_threshold) || (max_speed >= BaseLaunchControllerParams::launch_ready_speed_threshold)) + { + launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; + } + else if (pedalsData.accelPercent >= BaseLaunchControllerParams::launch_go_accel_threshold) + { + + initial_ecef_x_ = vn_data.ecef_coords[0]; + initial_ecef_y_ = vn_data.ecef_coords[1]; + initial_ecef_z_ = vn_data.ecef_coords[2]; + + launch_state_ = LaunchStates_e::LAUNCHING; + } + + // check accel above launch threshold and launch + break; + case LaunchStates_e::LAUNCHING: + { // use brackets to ignore 'cross initialization' of secs_since_launch + // check accel below launch threshold and brake above + if ((pedalsData.accelPercent <= BaseLaunchControllerParams::launch_stop_accel_threshold) || (pedalsData.brakePercent >= BaseLaunchControllerParams::launch_ready_brake_threshold)) + { + launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; + } + + calc_launch_algo(vn_data); + + writeout_.command.speeds_rpm[FL] = launch_speed_target_; + writeout_.command.speeds_rpm[FR] = launch_speed_target_; + writeout_.command.speeds_rpm[RL] = launch_speed_target_; + writeout_.command.speeds_rpm[RR] = launch_speed_target_; + + writeout_.command.torqueSetpoints[FL] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.torqueSetpoints[FR] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.torqueSetpoints[RL] = PhysicalParameters::AMK_MAX_TORQUE; + writeout_.command.torqueSetpoints[RR] = PhysicalParameters::AMK_MAX_TORQUE; + + break; + } + default: + break; + } + } +} + +TorqueControllerOutput_s BaseLaunchController::evaluate(const SharedCarState_s &state) +{ + tick(state.systick, state.pedals_data, state.drivetrain_data.measuredSpeeds, state.vn_data); + return writeout_; +} \ No newline at end of file diff --git a/lib/systems/src/Controllers/LaunchControllerAlgos.cpp b/lib/systems/src/Controllers/LaunchControllerAlgos.cpp new file mode 100644 index 000000000..dbc559e73 --- /dev/null +++ b/lib/systems/src/Controllers/LaunchControllerAlgos.cpp @@ -0,0 +1,67 @@ +#include "Controllers/LookupLaunchController.h" +#include "Controllers/SlipLaunchController.h" +#include "Controllers/SimpleLaunchController.h" + + +void TorqueControllerSimpleLaunch::calc_launch_algo(const VectornavData_s &vn_data) +{ + /* + Stolen launch algo from HT07. This ramps up the speed target over time. + launch rate target is m/s^2 and is the target acceleration rate + secs_since_launch takes the milliseconds since launch started and converts to sec + This is then converted to RPM for a speed target + There is an initial speed target that is your iitial instant acceleration on the wheels + */ + float secs_since_launch = (float)(current_millis_ - time_of_launch_) / 1000.0; + launch_speed_target_ = (int16_t)((float)secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); + launch_speed_target_ += init_speed_target_; + launch_speed_target_ = std::min((int)PhysicalParameters::AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); +} + +void TorqueControllerLookupLaunch::calc_launch_algo(const VectornavData_s &vn_data) +{ + + launch_speed_target_ = std::max((float)LookupLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); + + double dx = vn_data.ecef_coords[0] - initial_ecef_x_; + double dy = vn_data.ecef_coords[1] - initial_ecef_y_; + double dz = vn_data.ecef_coords[2] - initial_ecef_z_; + + double distance = sqrt((dx * dx) + (dy * dy) + (dz * dz)); + + /* + Distance-lookup launch algorithm. Takes in the vel_dist_lookup + generated from Luke's matlab/symlink to set speed targets based + on distance travelled from the start point. + This can also and may be better to replace with an integration + of body velocity. + */ + + uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10 + idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float))); + float mps_target = vel_dist_lookup[idx]; + + float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; + launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); +} +void TorqueControllerSlipLaunch::calc_launch_algo(const VectornavData_s &vn_data) +{ + // accelerate at constant speed for a period of time to get body velocity up + // may want to make this the ht07 launch algo + + // makes sure that the car launches at the target launch speed + launch_speed_target_ = std::max(launch_speed_target_, (float)SlipLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); + + /* + New slip-ratio based launch algorithm by Luke Chen. The basic idea + is to always be pushing the car a certain 'slip_ratio_' faster than + the car is currently going, theoretically always keeping the car in slip + */ + // m/s + float new_speed_target = (1 + slip_ratio_) * (vn_data.velocity_x); + // rpm + new_speed_target *= METERS_PER_SECOND_TO_RPM; + // makes sure the car target speed never goes lower than prev. target + // allows for the vn to 'spool' up and us to get reliable vx data + launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); +} \ No newline at end of file diff --git a/lib/systems/src/Controllers/LoadCellVectoringController.cpp b/lib/systems/src/Controllers/LoadCellVectoringController.cpp new file mode 100644 index 000000000..e85502e84 --- /dev/null +++ b/lib/systems/src/Controllers/LoadCellVectoringController.cpp @@ -0,0 +1,100 @@ +#include "Controllers/LoadCellVectoringController.h" + + +void TorqueControllerLoadCellVectoring::tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + const LoadCellInterfaceOutput_s &loadCellData +) +{ + // Calculate torque commands at 100hz + if (tick.triggers.trigger100) + { + // Apply FIR filter to load cell data + loadCellForcesRaw_[0][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FL; + loadCellForcesRaw_[1][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FR; + loadCellForcesRaw_[2][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RL; + loadCellForcesRaw_[3][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RR; + + for (int i = 0; i < 4; i++) + { + loadCellForcesFiltered_[i] = 0.0f; + for (int FIROffset = 0; FIROffset < numFIRTaps_; FIROffset++) + { + int index = (FIRCircBufferHead + FIROffset) % numFIRTaps_; + loadCellForcesFiltered_[i] += loadCellForcesRaw_[i][index] * FIRTaps_[FIROffset]; + } + } + FIRCircBufferHead = (FIRCircBufferHead + 1) % numFIRTaps_; + if (FIRCircBufferHead == numFIRTaps_ - 1) + FIRSaturated_ = true; + + // Do sanity checks on raw data + loadCellsErrorCounter_[0] = loadCellData.loadCellConversions.FL.raw != 4095 && loadCellData.loadCellConversions.FL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; + loadCellsErrorCounter_[1] = loadCellData.loadCellConversions.FR.raw != 4095 && loadCellData.loadCellConversions.FR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; + loadCellsErrorCounter_[2] = loadCellData.loadCellConversions.RL.raw != 4095 && loadCellData.loadCellConversions.RL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; + loadCellsErrorCounter_[3] = loadCellData.loadCellConversions.RR.raw != 4095 && loadCellData.loadCellConversions.RR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; + ready_ = FIRSaturated_ && loadCellsErrorCounter_[0] < errorCountThreshold_ && loadCellsErrorCounter_[1] < errorCountThreshold_ && loadCellsErrorCounter_[2] < errorCountThreshold_ && loadCellsErrorCounter_[3] < errorCountThreshold_; + + writeout_.ready = ready_; + + if (ready_) + { + // Calculate total normal force + float sumNormalForce = 0.0f; + for (int i = 0; i < 4; i++) + { + sumNormalForce += loadCellForcesFiltered_[i]; + } + + // Both pedals are not pressed and no implausibility has been detected + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torquePool; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + // NOTE: using "torquePool" here instead of torqueRequest for legibility + torquePool = accelRequest * PhysicalParameters::AMK_MAX_TORQUE * 4; + + writeout_.command.speeds_rpm[FL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[RL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[RR] = PhysicalParameters::AMK_MAX_RPM; + + writeout_.command.torqueSetpoints[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; + writeout_.command.torqueSetpoints[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; + writeout_.command.torqueSetpoints[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; + writeout_.command.torqueSetpoints[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; + } + else + { + // Negative torque request + // No load cell vectoring on regen + torqueRequest = PhysicalParameters::MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + writeout_.command.speeds_rpm[RL] = 0.0; + writeout_.command.speeds_rpm[RR] = 0.0; + + writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + } + } + else + { + writeout_.command = BaseControllerParams::TC_COMMAND_NO_TORQUE; + } + } +} + +TorqueControllerOutput_s TorqueControllerLoadCellVectoring::evaluate(const SharedCarState_s &state) +{ + tick(state.systick, state.pedals_data, state.loadcell_data); + return writeout_; +} \ No newline at end of file diff --git a/lib/systems/src/Controllers/SimpleController.cpp b/lib/systems/src/Controllers/SimpleController.cpp new file mode 100644 index 000000000..4906e4ed8 --- /dev/null +++ b/lib/systems/src/Controllers/SimpleController.cpp @@ -0,0 +1,47 @@ +#include "Controllers/SimpleController.h" + +void TorqueControllerSimple::tick(const PedalsSystemData_s &pedalsData) +{ + + // Both pedals are not pressed and no implausibility has been detected + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + torqueRequest = accelRequest * PhysicalParameters::AMK_MAX_TORQUE; + + writeout_.command.speeds_rpm[FL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[RL] = PhysicalParameters::AMK_MAX_RPM; + writeout_.command.speeds_rpm[RR] = PhysicalParameters::AMK_MAX_RPM; + + writeout_.command.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; + writeout_.command.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; + writeout_.command.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; + writeout_.command.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; + } + else + { + // Negative torque request + torqueRequest = PhysicalParameters::MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + writeout_.command.speeds_rpm[RL] = 0.0; + writeout_.command.speeds_rpm[RR] = 0.0; + + writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + } +} + +TorqueControllerOutput_s TorqueControllerSimple::evaluate(const SharedCarState_s &state) +{ + tick(state.pedals_data); + return writeout_; +} \ No newline at end of file diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp deleted file mode 100644 index e6e04fa8b..000000000 --- a/lib/systems/src/TorqueControllerMux.cpp +++ /dev/null @@ -1,315 +0,0 @@ -#include "TorqueControllerMux.h" -#include "Utility.h" -#include "PhysicalParameters.h" - -void TorqueControllerMux::tick( - const SysTick_s &tick, - const DrivetrainDynamicReport_s &drivetrainData, - const PedalsSystemData_s &pedalsData, - const SteeringSystemData_s &steeringData, - const LoadCellInterfaceOutput_s &loadCellData, - DialMode_e dashboardDialMode, - float accDerateFactor, - bool dashboardTorqueModeButtonPressed, - const vector_nav &vn_data, - const DrivetrainCommand_s &CASECommand) -{ - // Tick all torque controllers - torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]); - torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadCellData); - torqueControllerSimpleLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); - torqueControllerSlipLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds, &vn_data); - tcCASEWrapper_.tick( - (TCCaseWrapperTick_s){ - .command = CASECommand, - .steeringData = steeringData}); - - // Tick torque button logic at 50hz - if (tick.triggers.trigger50) - { - // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms - if (torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) - { - // WOW C++ is ass - torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); - torqueLimitButtonPressedTime_ = tick.millis; - } - torqueLimitButtonPressed_ = dashboardTorqueModeButtonPressed; - } - - // Tick TCMUX at 100hz - if (tick.triggers.trigger100) - { - // Determine next state based on dashboard dial state - // Only transisiton to the next state if: - // Vehicle speed is below 5 m/s - // AND torque delta between old and new controllers is < 0.5nm on every wheel - // AND the selected torque controller is in the ready state - if (muxMode_ != dialModeMap_[dashboardDialMode]) - { - // Check if speed permits mode change - bool speedPreventsModeChange = false; - for (int i = 0; i < NUM_MOTORS; i++) - { - speedPreventsModeChange |= drivetrainData.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND >= MAX_SPEED_FOR_MODE_CHANGE; - } - - // Check if torque delta permits mode change - bool torqueDeltaPreventsModeChange = false; - for (int i = 0; i < NUM_MOTORS; i++) - { - float torqueDelta = abs( - controllerOutputs_[static_cast(muxMode_)].command.torqueSetpoints[i] - controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].command.torqueSetpoints[i]); - - if (torqueDelta > MAX_TORQUE_DELTA_FOR_MODE_CHANGE) - { - torqueDeltaPreventsModeChange = true; - break; - } - } - - // Check if targeted controller is ready to be selected - bool controllerNotReadyPreventsModeChange = (controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].ready == false); - - if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) - { - muxMode_ = dialModeMap_[dashboardDialMode]; - 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 - // When the car goes below 5m/s, it will attempt to re-engage the faulted controller - // It will stay in safe mode if the controller is still faulted - if (controllerOutputs_[static_cast(muxMode_)].ready == false) - { - muxMode_ = TorqueController_e::TC_SAFE_MODE; - } - - // 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 - // 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(); - } - -} - -/* - Apply limit to make sure that regenerative braking is not applied when - wheelspeed is below 5kph on all wheels. - - FSAE rules: - EV.3.3.3 The powertrain must not regenerate energy when vehicle speed is between 0 and 5 km/hr - Assumption: - Assuming there won't be a scenario where there are positive and negative setpoints simultaneously - AND vehicle speed is < 5km/h -*/ -void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) -{ - const float noRegenLimitKPH = 10.0; - const float fullRegenLimitKPH = 5.0; - float maxWheelSpeed = 0.0; - float torqueScaleDown = 0.0; - bool allWheelsRegen = true; // true when all wheels are targeting speeds below the current wheel speed - - for (int i = 0; i < NUM_MOTORS; i++) - { -#ifdef ARDUINO_TEENSY41 - maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); - allWheelsRegen &= (command->speeds_rpm[i] < abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); -#else - maxWheelSpeed = std::max(maxWheelSpeed, std::abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR); - allWheelsRegen &= (command->speeds_rpm[i] < std::abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0); -#endif - } - - // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH - // linearly interpolate the scale factor between noRegenLimitKPH and fullRegenLimitKPH - torqueScaleDown = std::min(1.0f, std::max(0.0f, (maxWheelSpeed - fullRegenLimitKPH) / (noRegenLimitKPH - fullRegenLimitKPH))); - - if (allWheelsRegen) - { - for (int i = 0; i < NUM_MOTORS; i++) - { - command->torqueSetpoints[i] *= torqueScaleDown; - } - } -} - -/* - Apply 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 - preserve functionality of torque controllers -*/ -void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain) -{ - float net_torque_mag = 0; - float net_power = 0; - - // calculate current mechanical power - for (int i = 0; i < NUM_MOTORS; i++) - { -// get the total magnitude of torque from all 4 wheels -#ifdef ARDUINO_TEENSY41 // screw arduino.h macros - net_torque_mag += abs(command->torqueSetpoints[i]); - net_power += abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#else - // sum up net torque - net_torque_mag += std::abs(command->torqueSetpoints[i]); - // calculate P = T*w for each wheel and sum together - net_power += std::abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#endif - } - - // only evaluate power limit if current power exceeds it - if (net_power > (MAX_POWER_LIMIT)) - { - for (int i = 0; i < NUM_MOTORS; i++) - { - - // TODO: SOMEBODY PLZ MAKE THIS WORK - // enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_); - - // calculate the percent of total torque requested per wheel - float torque_percent = abs(command->torqueSetpoints[i] / net_torque_mag); - // based on the torque percent and max power limit, get the max power each wheel can use - float power_per_corner = (torque_percent * MAX_POWER_LIMIT); - -// power / omega (motor rad/s) to get torque per wheel -#ifdef ARDUINO_TEENSY41 - command->torqueSetpoints[i] = abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#else - command->torqueSetpoints[i] = std::abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); -#endif - command->torqueSetpoints[i] = std::max(0.0f, std::min(command->torqueSetpoints[i], getMaxTorque())); - } - } -} - -/* - Apply limit such that the average wheel torque is never above the max - torque allowed in the current torque mode. - This will uniformally scale down all torques as to not affect the functionality - of non-symmetrical torque controllers. -*/ -void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s *command) -{ - float max_torque = getMaxTorque(); - float avg_torque = 0; - - // get the average torque accross all 4 wheels - for (int i = 0; i < NUM_MOTORS; i++) - { -#ifdef ARDUINO_TEENSY41 // screw arduino.h macros - avg_torque += abs(command->torqueSetpoints[i]); -#else - avg_torque += std::abs(command->torqueSetpoints[i]); -#endif - } - avg_torque /= NUM_MOTORS; - - // if this is greather than the torque limit, scale down - if (avg_torque > max_torque) - { - // get the scale of avg torque above max torque - float scale = avg_torque / max_torque; - // divide by scale to lower avg below max torque - for (int i = 0; i < NUM_MOTORS; i++) - { - command->torqueSetpoints[i] /= scale; - } - } - - for (int i = 0; i < NUM_MOTORS; i++) - { - command->torqueSetpoints[i] = std::min(command->torqueSetpoints[i], max_torque); - } -} - -/** - * Apply limit such that wheelspeed never goes negative - */ -void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command) -{ - for (int i = 0; i < NUM_MOTORS; i++) - { - command->speeds_rpm[i] = std::max(0.0f, command->speeds_rpm[i]); - } -} - -/** - * 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 deleted file mode 100644 index 9497c9b61..000000000 --- a/lib/systems/src/TorqueControllers.cpp +++ /dev/null @@ -1,353 +0,0 @@ -#include "TorqueControllers.h" -#include "Utility.h" -#include -#include "PhysicalParameters.h" - -// TorqueControllerSimple - -void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit) -{ - - // Calculate torque commands at 100hz - if (tick.triggers.trigger100) - { - // Both pedals are not pressed and no implausibility has been detected - // accelRequest goes between 1.0 and -1.0 - float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; - float torqueRequest; - - if (accelRequest >= 0.0) - { - // Positive torque request - torqueRequest = accelRequest * AMK_MAX_TORQUE; - - // writeout_.command.speeds_rpm[FL] = accelRequest * AMK_MAX_RPM; - // writeout_.command.speeds_rpm[FR] = accelRequest * AMK_MAX_RPM;pid_input_ - // writeout_.command.speeds_rpm[RL] = accelRequest * AMK_MAX_RPM; - // writeout_.command.speeds_rpm[RR] = accelRequest * AMK_MAX_RPM; - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; - - writeout_.command.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; - writeout_.command.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; - writeout_.command.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; - writeout_.command.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; - } - else - { - // Negative torque request - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; - float netTorqueRequest = torqueRequest * 4; - - 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] = (netTorqueRequest * frontRegenTorqueScale_) / 2; - writeout_.command.torqueSetpoints[FR] = (netTorqueRequest * frontRegenTorqueScale_) / 2; - writeout_.command.torqueSetpoints[RL] = (netTorqueRequest * rearRegenTorqueScale_) / 2; - writeout_.command.torqueSetpoints[RR] = (netTorqueRequest * rearRegenTorqueScale_) / 2; - } - } -} - -// TorqueControllerLoadCellVectoring - -void TorqueControllerLoadCellVectoring::tick( - const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - float torqueLimit, - const LoadCellInterfaceOutput_s &loadCellData -) -{ - // Calculate torque commands at 100hz - if (tick.triggers.trigger100) - { - // Apply FIR filter to load cell data - loadCellForcesRaw_[0][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FL; - loadCellForcesRaw_[1][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.FR; - loadCellForcesRaw_[2][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RL; - loadCellForcesRaw_[3][FIRCircBufferHead] = loadCellData.loadCellForcesFiltered.RR; - - for (int i = 0; i < 4; i++) - { - loadCellForcesFiltered_[i] = 0.0f; - for (int FIROffset = 0; FIROffset < numFIRTaps_; FIROffset++) - { - int index = (FIRCircBufferHead + FIROffset) % numFIRTaps_; - loadCellForcesFiltered_[i] += loadCellForcesRaw_[i][index] * FIRTaps_[FIROffset]; - } - } - FIRCircBufferHead = (FIRCircBufferHead + 1) % numFIRTaps_; - if (FIRCircBufferHead == numFIRTaps_ - 1) - FIRSaturated_ = true; - - // Do sanity checks on raw data - loadCellsErrorCounter_[0] = loadCellData.loadCellConversions.FL.raw != 4095 && loadCellData.loadCellConversions.FL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; - loadCellsErrorCounter_[1] = loadCellData.loadCellConversions.FR.raw != 4095 && loadCellData.loadCellConversions.FR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; - loadCellsErrorCounter_[2] = loadCellData.loadCellConversions.RL.raw != 4095 && loadCellData.loadCellConversions.RL.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; - loadCellsErrorCounter_[3] = loadCellData.loadCellConversions.RR.raw != 4095 && loadCellData.loadCellConversions.RR.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; - ready_ = FIRSaturated_ && loadCellsErrorCounter_[0] < errorCountThreshold_ && loadCellsErrorCounter_[1] < errorCountThreshold_ && loadCellsErrorCounter_[2] < errorCountThreshold_ && loadCellsErrorCounter_[3] < errorCountThreshold_; - - writeout_.ready = ready_; - - if (ready_) - { - // Calculate total normal force - float sumNormalForce = 0.0f; - for (int i = 0; i < 4; i++) - { - sumNormalForce += loadCellForcesFiltered_[i]; - } - - // Both pedals are not pressed and no implausibility has been detected - // accelRequest goes between 1.0 and -1.0 - float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; - float torquePool; - float torqueRequest; - - if (accelRequest >= 0.0) - { - // Positive torque request - // NOTE: using "torquePool" here instead of torqueRequest for legibility - torquePool = accelRequest * AMK_MAX_TORQUE * 4; - - writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; - writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; - - writeout_.command.torqueSetpoints[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; - writeout_.command.torqueSetpoints[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; - writeout_.command.torqueSetpoints[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; - writeout_.command.torqueSetpoints[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; - } - else - { - // Negative torque request - - 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; - - float normalForcePercentFL = loadCellForcesFiltered_[0] / sumNormalForce; - float normalForcePercentFR = loadCellForcesFiltered_[1] / sumNormalForce; - float normalForcePercentRL = loadCellForcesFiltered_[2] / sumNormalForce; - float normalForcePercentRR = loadCellForcesFiltered_[3] / sumNormalForce; - - if (normalForcePercentFL + normalForcePercentFR < .5 || normalForcePercentRL + normalForcePercentRR > .5) { - normalForcePercentFL = .25; - normalForcePercentFR = .25; - normalForcePercentRL = .25; - normalForcePercentRR = .25; - } - - writeout_.command.torqueSetpoints[FL] = torquePool * frontRegenTorqueScale_ * normalForcePercentFL; - writeout_.command.torqueSetpoints[FR] = torquePool * frontRegenTorqueScale_ * normalForcePercentFR; - writeout_.command.torqueSetpoints[RL] = torquePool * rearRegenTorqueScale_ * normalForcePercentRL; - writeout_.command.torqueSetpoints[RR] = torquePool * rearRegenTorqueScale_ * normalForcePercentRR; - - // 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 - { - writeout_.command = TC_COMMAND_NO_TORQUE; - } - } -} - -void BaseLaunchController::tick( - const SysTick_s &tick, - const PedalsSystemData_s &pedalsData, - const float wheel_rpms[], - const vector_nav* vn_data) -{ - - if (tick.triggers.trigger100){ - - current_millis_ = tick.millis; - - int16_t brake_torque_req = pedalsData.regenPercent * MAX_REGEN_TORQUE; - - float max_speed = 0; - for(int i = 0; i < 4; i++){ - max_speed = std::max(max_speed, abs(wheel_rpms[i])); - } - - writeout_.ready = true; - - switch(launch_state_){ - case LaunchStates_e::LAUNCH_NOT_READY: - // set torques and speed to 0 - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - writeout_.command.speeds_rpm[RL] = 0.0; - writeout_.command.speeds_rpm[RR] = 0.0; - - writeout_.command.torqueSetpoints[FL] = brake_torque_req; - writeout_.command.torqueSetpoints[FR] = brake_torque_req; - writeout_.command.torqueSetpoints[RL] = brake_torque_req; - writeout_.command.torqueSetpoints[RR] = brake_torque_req; - - //init launch vars - launch_speed_target_ = 0; - time_of_launch_ = tick.millis; - // check speed is 0 and pedals not pressed - if((pedalsData.accelPercent < launch_ready_accel_threshold) - && (pedalsData.brakePercent < launch_ready_brake_threshold) - && (max_speed < launch_ready_speed_threshold)) - { - launch_state_ = LaunchStates_e::LAUNCH_READY; - } - - break; - case LaunchStates_e::LAUNCH_READY: - // set torques and speed to 0 - writeout_.command.speeds_rpm[FL] = 0.0; - writeout_.command.speeds_rpm[FR] = 0.0; - writeout_.command.speeds_rpm[RL] = 0.0; - writeout_.command.speeds_rpm[RR] = 0.0; - - writeout_.command.torqueSetpoints[FL] = brake_torque_req; - writeout_.command.torqueSetpoints[FR] = brake_torque_req; - writeout_.command.torqueSetpoints[RL] = brake_torque_req; - writeout_.command.torqueSetpoints[RR] = brake_torque_req; - - //init launch vars - launch_speed_target_ = 0; - time_of_launch_ = current_millis_; - - //check speed is 0 and brake not pressed - if ((pedalsData.brakePercent >= launch_ready_brake_threshold) - || (max_speed >= launch_ready_speed_threshold)) - { - launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; - } else if(pedalsData.accelPercent >= launch_go_accel_threshold){ - - initial_ecef_x_ = vn_data->ecef_coords[0]; - initial_ecef_y_ = vn_data->ecef_coords[1]; - initial_ecef_z_ = vn_data->ecef_coords[2]; - - launch_state_ = LaunchStates_e::LAUNCHING; - } - - //check accel above launch threshold and launch - break; - case LaunchStates_e::LAUNCHING: - { // use brackets to ignore 'cross initialization' of secs_since_launch - //check accel below launch threshold and brake above - if((pedalsData.accelPercent <= launch_stop_accel_threshold) - || (pedalsData.brakePercent >= launch_ready_brake_threshold)) - { - launch_state_ = LaunchStates_e::LAUNCH_NOT_READY; - } - - calc_launch_algo(vn_data); - - writeout_.command.speeds_rpm[FL] = launch_speed_target_; - writeout_.command.speeds_rpm[FR] = launch_speed_target_; - writeout_.command.speeds_rpm[RL] = launch_speed_target_; - writeout_.command.speeds_rpm[RR] = launch_speed_target_; - - writeout_.command.torqueSetpoints[FL] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[FR] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RL] = AMK_MAX_TORQUE; - writeout_.command.torqueSetpoints[RR] = AMK_MAX_TORQUE; - - } - break; - default: - break; - - } - - } -} - -void TorqueControllerSimpleLaunch::calc_launch_algo(const vector_nav* vn_data) { - /* - Stolen launch algo from HT07. This ramps up the speed target over time. - launch rate target is m/s^2 and is the target acceleration rate - secs_since_launch takes the milliseconds since launch started and converts to sec - This is then converted to RPM for a speed target - There is an initial speed target that is your iitial instant acceleration on the wheels - */ - float secs_since_launch = (float)(current_millis_ - time_of_launch_) / 1000.0; - launch_speed_target_ = (int16_t)((float) secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM); - launch_speed_target_ += init_speed_target_; - launch_speed_target_ = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target_)); -} - -void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) { - // accelerate at constant speed for a period of time to get body velocity up - // may want to make this the ht07 launch algo - - // makes sure that the car launches at the target launch speed - launch_speed_target_ = std::max(launch_speed_target_, (float)DEFAULT_LAUNCH_SPEED_TARGET); - - /* - New slip-ratio based launch algorithm by Luke Chen. The basic idea - is to always be pushing the car a certain 'slip_ratio_' faster than - the car is currently going, theoretically always keeping the car in slip - */ - // m/s - float new_speed_target = (1 + slip_ratio_) * (vn_data->velocity_x); - // rpm - new_speed_target *= METERS_PER_SECOND_TO_RPM; - // makes sure the car target speed never goes lower than prev. target - // allows for the vn to 'spool' up and us to get reliable vx data - launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); -} - -void TorqueControllerLookupLaunch::calc_launch_algo(const vector_nav* vn_data) { - - launch_speed_target_ = std::max((float)DEFAULT_LAUNCH_SPEED_TARGET, launch_speed_target_); - - double dx = vn_data->ecef_coords[0] - initial_ecef_x_; - double dy = vn_data->ecef_coords[1] - initial_ecef_y_; - double dz = vn_data->ecef_coords[2] - initial_ecef_z_; - - double distance = sqrt((dx*dx) + (dy*dy) + (dz*dz)); - - /* - Distance-lookup launch algorithm. Takes in the vel_dist_lookup - generated from Luke's matlab/symlink to set speed targets based - on distance travelled from the start point. - This can also and may be better to replace with an integration - of body velocity. - */ - - uint32_t idx = (uint32_t)(distance * 10); // multiply by 10 to be used as index for meters in steps of 1/10 - idx = std::min(idx, (uint32_t)(sizeof(vel_dist_lookup) / sizeof(float))); - float mps_target = vel_dist_lookup[idx]; - - float new_speed_target = mps_target * METERS_PER_SECOND_TO_RPM; - launch_speed_target_ = std::max(launch_speed_target_, new_speed_target); - -} - -void TorqueControllerCASEWrapper::tick(const TCCaseWrapperTick_s &intake) -{ - - for (int i = 0; i < NUM_MOTORS; i++) - { - writeout_.command.speeds_rpm[i] = intake.command.speeds_rpm[i]; - writeout_.command.torqueSetpoints[i] = intake.command.torqueSetpoints[i]; - } - - writeout_.ready = intake.steeringData.status != SteeringSystemStatus_e::STEERING_SYSTEM_ERROR; - -} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 6d85fc724..a9474fcd6 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,3 +1,10 @@ +[common] +lib_deps_shared = + Nanopb + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 + https://github.com/hytech-racing/shared_firmware_types.git#feb3b83 + https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 + [env:test_env] platform = native test_framework = googletest @@ -11,13 +18,8 @@ lib_ignore = test_ignore= test_interfaces* lib_deps= - SPI - Nanopb - https://github.com/vjmuzik/NativeEthernet.git - 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 - https://github.com/hytech-racing/shared_firmware_systems.git#af96a63 - + ${common.lib_deps_shared} + [env:teensy41] ; Testing variables test_framework=unity @@ -50,22 +52,15 @@ monitor_speed = 115200 upload_protocol = teensy-cli extra_scripts = pre:extra_script.py lib_deps = - SPI - Nanopb - https://github.com/vjmuzik/NativeEthernet.git - https://github.com/hytech-racing/HT_params/releases/download/2024-05-07T06_59_33/ht_eth_pb_lib.tar.gz - https://github.com/hytech-racing/shared_firmware_interfaces.git#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/108/can_lib.tar.gz - git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 - -[env:test_interfaces] -; Testing variables -test_framework=unity -test_ignore=test_systems* + ${common.lib_deps_shared} + SPI + https://github.com/hytech-racing/shared_firmware_interfaces.git#shared_types + https://github.com/tonton81/FlexCAN_T4 + https://github.com/ssilverman/QNEthernet#v0.26.0 + https://github.com/RCMast3r/hytech_can#testing_new_inv_ids + https://github.com/hytech-racing/HT_CAN/releases/download/125/can_lib.tar.gz + +[env:test_can_on_teensy] lib_ignore = mock_interfaces mock_systems diff --git a/src/main.cpp b/src/main.cpp index 9af88b81d..a8c2fe435 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,12 +1,11 @@ /* Include files */ /* System Includes*/ #include -#include "ParameterInterface.h" /* Libraries */ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" #include "MCU_rev15_defs.h" -// #include "NativeEthernet.h" +#include "PrintLogger.h" // /* Interfaces */ @@ -32,8 +31,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" @@ -172,8 +172,7 @@ OrbisBR10 steering1(&Serial5); // /* // INTERFACES // */ -ParameterInterface param_interface; -ETHInterfaces ethernet_interfaces = {¶m_interface}; +ETHInterfaces ethernet_interfaces = {}; VNInterface vn_interface(&CAN3_txBuffer); DashboardInterface dashboard(&CAN3_txBuffer); AMSInterface ams_interface(&CAN3_txBuffer, SOFTWARE_OK); @@ -212,7 +211,6 @@ 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(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 @@ -291,8 +289,26 @@ CASEConfiguration case_config = { .max_regen_torque = 21.42, .max_torque = 21.42, }; - +RateLimitedLogger logger; +//// Controllers CASESystem case_system(&CAN3_txBuffer, 100, 70, 550, case_config); +// mode 0 +TorqueControllerSimple tc_simple(1.0f, 1.0f); +// mode 1 +TorqueControllerLoadCellVectoring tc_vec; +// mode 2 +TorqueControllerCASEWrapper case_wrapper(&case_system); + +// mode 3 +TorqueControllerSimpleLaunch simple_launch; +// mode 4 +TorqueControllerSlipLaunch slip_launch; +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}); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); @@ -316,7 +332,6 @@ void tick_all_systems(const SysTick_s ¤t_system_tick); /* Reset inverters */ void drivetrain_reset(); -void handle_ethernet_interface_comms(); /* SETUP @@ -331,11 +346,6 @@ void setup() // protobuf_send_socket.begin(EthParams::default_protobuf_send_port); // protobuf_recv_socket.begin(EthParams::default_protobuf_recv_port); - /* Do this to send message VVV */ - // protobuf_socket.beginPacket(EthParams::default_TCU_ip, EthParams::default_protobuf_port); - // protobuf_socket.write(buf, len); - // protobuf_socker.endPacket(); - SPI.begin(); a1.init(); a2.init(); @@ -396,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); @@ -406,8 +414,19 @@ void loop() tick_all_interfaces(curr_tick); // tick systems - tick_all_systems(curr_tick); + // 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()); + + 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()) @@ -415,10 +434,8 @@ void loop() drivetrain.reset_drivetrain(); } // tick state machine - fsm.tick_state_machine(curr_tick.millis); - // give the state of the car to the param interface - param_interface.update_car_state(fsm.get_state()); + fsm.tick_state_machine(curr_tick.millis, car_state_inst); // tick safety system safety_system.software_shutdown(curr_tick); @@ -463,8 +480,11 @@ void loop() Serial.println(ams_interface.get_filtered_min_cell_voltage()); Serial.print("Filtered max cell temp: "); Serial.println(ams_interface.get_filtered_max_cell_temp()); + Serial.print("Current TC index: "); + Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_controller_mode)); + Serial.print("Current TC error: "); + Serial.println(static_cast(torque_controller_mux.get_tc_mux_status().current_error)); Serial.println(); - Serial.println(); } @@ -510,19 +530,17 @@ 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.getTorqueLimit(), + 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.activeController()->get_launch_state()), + 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.getDriveMode()), - static_cast(torque_controller_mux.getTorqueLimit()), - torque_controller_mux.getMaxTorque(), + torque_controller_mux.get_tc_mux_status(), buzzer.buzzer_is_on(), pedals_system.getPedalsSystemData(), ams_interface.pack_charge_is_critical(), @@ -548,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); } if (t.trigger50) // 50Hz @@ -636,7 +654,7 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) drivetrain.tick(current_system_tick); // // tick torque controller mux - DrivetrainCommand_s controller_output = case_system.evaluate( + auto __attribute__((unused)) case_status = case_system.evaluate( current_system_tick, vn_interface.get_vn_struct(), steering_system.getSteeringSystemData(), @@ -648,38 +666,4 @@ void tick_all_systems(const SysTick_s ¤t_system_tick) dashboard.startButtonPressed(), vn_interface.get_vn_struct().vn_status); - // case_system.update_config_from_param_interface(param_interface); - - torque_controller_mux.tick( - current_system_tick, - drivetrain.get_dynamic_data(), - pedals_system.getPedalsSystemData(), - steering_system.getSteeringSystemData(), - load_cell_interface.getLoadCellForces(), - dashboard.getDialMode(), - ams_interface.get_acc_derate_factor(), - dashboard.torqueModeButtonPressed(), - vn_interface.get_vn_struct(), - controller_output); } - -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); - - // this is just kinda here i know. - if (param_interface.params_need_sending()) - { - // Serial.println("handling ethernet"); - auto config = param_interface.get_config(); - if (!handle_ethernet_socket_send_pb(&protobuf_send_socket, config, config_fields)) - { - // TODO this means that something bad has happend - } - param_interface.reset_params_need_sending(); - } -} \ No newline at end of file diff --git a/test/test_systems/fake_controller_type.h b/test/test_systems/fake_controller_type.h new file mode 100644 index 000000000..d27fa367d --- /dev/null +++ b/test/test_systems/fake_controller_type.h @@ -0,0 +1,15 @@ +#ifndef __FAKE_CONTROLLER_TYPE_H__ +#define __FAKE_CONTROLLER_TYPE_H__ +#include "BaseController.h" +struct DummyQueue_s +{ +}; + +class TestControllerType : public virtual Controller +{ + +public: + TorqueControllerOutput_s output; + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) override { return output; } +}; +#endif // __FAKE_CONTROLLER_TYPE_H__ \ No newline at end of file diff --git a/test/test_systems/launch_controller_integration_testing.h b/test/test_systems/launch_controller_integration_testing.h new file mode 100644 index 000000000..2004ead69 --- /dev/null +++ b/test/test_systems/launch_controller_integration_testing.h @@ -0,0 +1,184 @@ +// TODO put the launch controller testing in here with the tc mux + +#include "TorqueControllerMux.h" +#include "TorqueControllers.h" +#include "fake_controller_type.h" + +constexpr DrivetrainDynamicReport_s create_drive_report(float speed_fl, float speed_fr, float speed_rl, float speed_rr) +{ + DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { + .measuredInverterFLPackVoltage = 550, + .measuredSpeeds = { + speed_fl, + speed_fr, + speed_rl, + speed_rr}, + .measuredTorques = {0.0, 0.0, 0.0, 0.0}, + .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, + .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0}}; + return simulated_slow_drivetrain_dynamics; +} + +constexpr PedalsSystemData_s create_pedals_data(float accel_percent, float brake_percent) +{ + PedalsSystemData_s pedals = { + .accelImplausible = false, + .brakeImplausible = false, + .brakePressed = false, + .brakeAndAccelPressedImplausibility = false, + .implausibilityExceededMaxDuration = false, + .accelPercent = accel_percent, + .brakePercent = brake_percent}; + return pedals; +} + +constexpr float slow_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM; +constexpr auto simulated_slow_drivetrain_dynamics = create_drive_report(slow_speed, slow_speed, slow_speed, slow_speed); +constexpr auto simulated_barely_launch_drivetrain_dynamics = create_drive_report(0, 2785.86, 0, 0); +constexpr auto simulated_no_launch_drivetrain_dynamics = create_drive_report(0, 2786.86, 0, 0); +constexpr float fast_speed = (TC_MUX_DEFAULT_PARAMS::MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM; +constexpr auto simulated_fast_drivetrain_dynamics = create_drive_report(fast_speed, fast_speed, fast_speed, fast_speed); +constexpr auto simulated_full_accel_press = create_pedals_data(1.0, 0); +constexpr auto simulated_no_accel_press = create_pedals_data(0, 0); +constexpr auto simulated_accel_and_brake_press = create_pedals_data(1.0, 0.3); + +TEST(LaunchIntergationTesting, test_simple_launch_controller) +{ + + SysClock clock = SysClock(); + SysTick_s cur_tick; + cur_tick = clock.tick(0); + 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); + // 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}); + + auto res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_0, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + // tick again to calculate state switch + + // tick again to calculate state switch + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY); + + // press accelerator but with one wheel right at max speed threshold + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, no_launch_allowed_state); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY); + + // // go back to launch ready state + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, still_state); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_READY); + + // press accel now with one wheelspeed barely under threshold + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, barely_launch_state); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state); + ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02); + + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_3, TorqueLimit_e::TCMUX_FULL_TORQUE, one_sec_passed_in_launch_state_w_error); + ASSERT_EQ(simple_launch.get_launch_state(), LaunchStates_e::LAUNCH_NOT_READY); +} + +TEST(LaunchIntergationTesting, test_slip_launch_controller) +{ + SysClock clock = SysClock(); + SysTick_s cur_tick; + cur_tick = clock.tick(0); + + VectornavData_s vn_data{}; + // 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}); + + 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, {}); + 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, {}); + 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, {}); + res = torque_controller_mux.getDrivetrainCommand(ControllerMode_e::MODE_4, TorqueLimit_e::TCMUX_FULL_TORQUE, more_accel_time_since_launch); + + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + ASSERT_EQ(res.speeds_rpm[0], BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET); + + // // if velocity is less than the default speed, it should still go at launch speed + vn_data.velocity_x = 0; // m/s + 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 + + ASSERT_EQ(slip_launch.get_launch_state(), LaunchStates_e::LAUNCHING); + // // this is approx. the speed the car would be going at 1500 rpm + // // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this + vn_data.velocity_x = BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s + + 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); + printf("launch vx_body: %.2f\n", (float)res.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); + ASSERT_EQ((int16_t)res.speeds_rpm[0] > (BaseLaunchControllerParams::DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); + + // TODO this is actually failing, idk what is going on here @walker + // ASSERT_NEAR((float)((int)((res.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f, 0.02); +} \ No newline at end of file diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index a1d174e6d..75d2b93a8 100644 --- a/test/test_systems/main.cpp +++ b/test/test_systems/main.cpp @@ -2,11 +2,13 @@ #include #include "state_machine_test.h" -// #include "pedals_system_test.h" -#include "torque_controller_mux_test.h" +#include "pedals_system_test.h" +#include "launch_controller_integration_testing.h" +#include "tc_mux_v2_testing.h" #include "drivetrain_system_test.h" #include "safety_system_test.h" -// #include "test_CASE.h" +#include "test_CASE.h" + // #include "param_system_test.h" // #include "steering_system_test.h" diff --git a/test/test_systems/pedals_system_test.h b/test/test_systems/pedals_system_test.h index afbb8a121..dc0f19a1c 100644 --- a/test/test_systems/pedals_system_test.h +++ b/test/test_systems/pedals_system_test.h @@ -273,12 +273,15 @@ TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activati TEST(PedalsSystemTesting, test_implausibility_duration) { AnalogConversion_s test_accel1_val = {2583, 0.37, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_accel2_val = {1068, 0.37, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_brake_val = {1510, get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, 1510), AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel2_val = {1068, 1.00, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {1510, get_pedal_conversion_val(BRAKE1_PEDAL_MIN, BRAKE1_PEDAL_MAX, BRAKE1_PEDAL_MAX), AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; PedalsSystem pedals(get_real_accel_pedal_params(), get_real_brake_pedal_params()); auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); + + std::cout << data.accelPressed < #include #include "MCUStateMachine.h" +#include "fake_controller_type.h" - +class DumbController : public Controller +{ + TorqueControllerOutput_s evaluate(const SharedCarState_s &state) { return {}; } +}; class DrivetrainMock { public: @@ -24,9 +28,12 @@ class DrivetrainMock bool drivetrain_error_occured() { return drivetrain_error_; }; void command_drivetrain(const DrivetrainCommand_s &data){}; - void disable_no_pins() {}; + void disable_no_pins(){}; }; +SharedCarState_s dummy_state({}, {}, {}, {}, {}, {}); +DumbController c; + void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface) { // ticking without hv over threshold testing and ensuring the tractive system not active still @@ -34,11 +41,11 @@ void handle_startup(MCUStateMachine &state_machine, unsigned lon drivetrain.hv_thresh_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); // hv going over threshold -> tractive system active drivetrain.hv_thresh_ = true; sys_time2 += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time2 += 1; dash_interface.start_button_status_ = true; AnalogConversion_s pedals1_data; @@ -54,58 +61,60 @@ void handle_startup(MCUStateMachine &state_machine, unsigned lon auto pedals4_data = pedals3_data; pedals.tick(SysTick_s{}, pedals1_data, pedals2_data, pedals3_data, pedals4_data); // get to enabling inverters - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); } TEST(MCUStateMachineTesting, test_state_machine_init_tick) { - - AMSInterface ams(0,0,0,0,0,0); + + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; EXPECT_EQ(state_machine.get_state(), CAR_STATE::STARTUP); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } TEST(MCUStateMachineTesting, test_state_machine_tractive_system_activation) { - AMSInterface ams(0,0,0,0,0,0); + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); + SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); unsigned long sys_time = 1000; // ticking without hv over threshold testing and ensuring the tractive system not active still - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); // hv going over threshold -> tractive system drivetrain.hv_thresh_ = true; sys_time += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); // hv going under thresh -> tractive system not active drivetrain.hv_thresh_ = false; sys_time += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } @@ -113,29 +122,30 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) { unsigned long sys_time = 1000; - AMSInterface ams(0,0,0,0,0,0); + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); + ; SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); // ticking without hv over threshold testing and ensuring the tractive system not active still - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); sys_time += 1; EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); // hv going over threshold -> tractive system drivetrain.hv_thresh_ = true; sys_time += 1; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); sys_time += 1; @@ -153,21 +163,21 @@ TEST(MCUStateMachineTesting, test_state_machine_tractive_system_enabling) pedals3_data.status = pedals1_data.status; auto pedals4_data = pedals3_data; pedals.tick(SysTick_s{}, pedals1_data, pedals2_data, pedals3_data, pedals4_data); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); } // test getting into and out of the waiting RTD and ensuring it stays within the state when we want it to TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) { - AMSInterface ams(0,0,0,0,0,0); + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); @@ -177,31 +187,32 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert) drivetrain.hv_thresh_ = true; drivetrain.drivetrain_inited_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 20; dash_interface.buzzer = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 35; dash_interface.buzzer = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); } TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) { - AMSInterface ams(0,0,0,0,0,0); + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); + ; SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); @@ -213,14 +224,14 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) EXPECT_EQ(state_machine.get_state(), CAR_STATE::ENABLING_INVERTERS); drivetrain.drivetrain_inited_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); buzzer.activate_buzzer(sys_time); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 20; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } @@ -229,20 +240,19 @@ TEST(MCUStateMachineTesting, test_state_machine_ready_to_drive_alert_leaving) TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_active) { - AMSInterface ams(0,0,0,0,0,0); + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; drivetrain.drivetrain_error_ = false; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); + ; SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); - - unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); @@ -251,25 +261,25 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti sys_time += 70; drivetrain.drivetrain_inited_ = true; drivetrain.hv_thresh_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 70; drivetrain.hv_thresh_ = true; dash_interface.buzzer = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); dash_interface.buzzer = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); drivetrain.drivetrain_error_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_ACTIVE); } @@ -277,21 +287,20 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_acti TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_active) { - AMSInterface ams(0,0,0,0,0,0); + int mock; + AMSInterface ams(&mock, 0, 0, 0, 0, 0); BuzzerController buzzer(50); DrivetrainMock drivetrain; drivetrain.drivetrain_error_ = false; - PedalsSystem pedals({},{}); + PedalsSystem pedals({}, {}); DashboardInterface dash_interface; - TelemetryInterface telem_interface; - TorqueControllerMux tc_mux(&telem_interface); + TCMuxType tc_mux({static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c), static_cast(&c)}, {true, true, true, true, true}); + ; SafetySystem ss(&ams, 0); MCUStateMachine state_machine(&buzzer, &drivetrain, &dash_interface, &pedals, &tc_mux, &ss); - - - + unsigned long sys_time = 1000; handle_startup(state_machine, sys_time, drivetrain, pedals, dash_interface); @@ -300,26 +309,26 @@ TEST(MCUStateMachineTesting, test_state_machine_rtd_state_transitions_to_ts_not_ sys_time += 70; drivetrain.drivetrain_inited_ = true; drivetrain.hv_thresh_ = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::WAITING_READY_TO_DRIVE_SOUND); sys_time += 70; drivetrain.hv_thresh_ = true; dash_interface.buzzer = true; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); dash_interface.buzzer = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::READY_TO_DRIVE); // drivetrain.drivetrain_error_ = true; drivetrain.hv_thresh_ = false; - state_machine.tick_state_machine(sys_time); + state_machine.tick_state_machine(sys_time, dummy_state); EXPECT_EQ(state_machine.get_state(), CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); } diff --git a/test/test_systems/tc_mux_v2_testing.h b/test/test_systems/tc_mux_v2_testing.h new file mode 100644 index 000000000..7d7d1beae --- /dev/null +++ b/test/test_systems/tc_mux_v2_testing.h @@ -0,0 +1,283 @@ +#ifndef __TC_MUX_V2_TESTING_H__ +#define __TC_MUX_V2_TESTING_H__ + +#include "TorqueControllerMux.h" +#include "TorqueControllers.h" +#include "fake_controller_type.h" + + + +// TODO +// - [x] test to ensure that the size checking for desired modes works and failes properly +template +void set_four_outputs(quad_array_type &out, float val) +{ + out[0] = val; + out[1] = val; + out[2] = val; + out[3] = val; +} + +template +void set_outputs(controller_type &controller, float mps, float torque) +{ + controller.output.command.speeds_rpm[0] = METERS_PER_SECOND_TO_RPM * mps; + controller.output.command.speeds_rpm[1] = METERS_PER_SECOND_TO_RPM * mps; + controller.output.command.speeds_rpm[2] = METERS_PER_SECOND_TO_RPM * mps; + controller.output.command.speeds_rpm[3] = METERS_PER_SECOND_TO_RPM * mps; + controller.output.command.torqueSetpoints[0] = torque; + controller.output.command.torqueSetpoints[1] = torque; + controller.output.command.torqueSetpoints[2] = torque; + controller.output.command.torqueSetpoints[3] = torque; +} +template +void set_output_rpm(controller_type &controller, float rpm, float torque) +{ + controller.output.command.speeds_rpm[0] = rpm; + controller.output.command.speeds_rpm[1] = rpm; + controller.output.command.speeds_rpm[2] = rpm; + controller.output.command.speeds_rpm[3] = rpm; + controller.output.command.torqueSetpoints[0] = torque; + controller.output.command.torqueSetpoints[1] = torque; + controller.output.command.torqueSetpoints[2] = torque; + controller.output.command.torqueSetpoints[3] = torque; +} +TEST(TorqueControllerMuxTesting, test_construction) +{ + TestControllerType inst1, inst2; + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); +} + +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); + } +} + + +// ensure that swapping to a controller that has a higher desired output speed than previously +// commanded that we dont switch +TEST(TorqueControllerMuxTesting, test_controller_output_swap_logic) +{ + TestControllerType inst1, inst2; + set_outputs(inst1, 0, 1); + set_outputs(inst2, 6, 1); + 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, test_torque_diff_swap_limit) +{ + TestControllerType inst1, inst2; + set_outputs(inst1, 0.1, 1); + set_outputs(inst2, 3, 10); + TorqueControllerMux<2> test({static_cast(&inst1), static_cast(&inst2)}, {false, false}); + 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); + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + out1 = test.getDrivetrainCommand(ControllerMode_e::MODE_1, TorqueLimit_e::TCMUX_FULL_TORQUE, state); + + ASSERT_EQ(test.get_tc_mux_status().current_error, TorqueControllerMuxError::ERROR_TORQUE_DIFF_TOO_HIGH); + + ASSERT_EQ(test.get_tc_mux_status().current_controller_mode, ControllerMode_e::MODE_0); + + ASSERT_EQ(out1.torqueSetpoints[0], 1); + ASSERT_EQ(out1.torqueSetpoints[1], 1); + ASSERT_EQ(out1.torqueSetpoints[2], 1); + ASSERT_EQ(out1.torqueSetpoints[3], 1); +} + +TEST(TorqueControllerMuxTesting, test_construction_with_new_controller_orgs) +{ + // mode 0 + 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_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); + + // mode 3 + TorqueControllerSimpleLaunch simple_launch; + // mode 4 + TorqueControllerSlipLaunch slip_launch; + TorqueControllerMux<5> torque_controller_mux({static_cast(&tc_simple), + static_cast(&tc_vec), + static_cast(&case_wrapper), + static_cast(&simple_launch), + static_cast(&slip_launch)}, + {false, true, false, false, false}); + 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); +} + +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); + } + for (int i = 0; i < 4; i++) + { + 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) +{ + 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); + } + 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 diff --git a/test/test_systems/test_CASE.h b/test/test_systems/test_CASE.h index 50fb5c448..bbfa35563 100644 --- a/test/test_systems/test_CASE.h +++ b/test/test_systems/test_CASE.h @@ -20,7 +20,7 @@ TEST(CASESystemTesting, test_vn_start_time) CASEConfiguration case_config = { // Following used for generated code - .AbsoluteTorqueLimit = AMK_MAX_TORQUE, // N-m, Torque limit used for yaw pid torque split overflow + .AbsoluteTorqueLimit = PhysicalParameters::AMK_MAX_TORQUE, // N-m, Torque limit used for yaw pid torque split overflow .yaw_pid_p = 1.369, .yaw_pid_i = 0.25, .yaw_pid_d = 0.0, @@ -53,7 +53,7 @@ TEST(CASESystemTesting, test_vn_start_time) .yaw_pid_brakes_d = 0.0, .decoupledYawPIDBrakesMaxDIfference = 2, // N-m .discontinuousBrakesPercentThreshold = 0.4, - .TorqueMode = AMK_MAX_TORQUE, // N-m + .TorqueMode = PhysicalParameters::AMK_MAX_TORQUE, // N-m .RegenLimit = -10.0, // N-m .useNoRegen5kph = true, .useTorqueBias = true, @@ -67,9 +67,9 @@ TEST(CASESystemTesting, test_vn_start_time) .tcs_pid_upper_rpm_rear = 5000.0, // RPM // Following used for calculate_torque_request in CASESystem.tpp - .max_rpm = AMK_MAX_RPM, - .max_regen_torque = AMK_MAX_TORQUE, - .max_torque = AMK_MAX_TORQUE, + .max_rpm = PhysicalParameters::AMK_MAX_RPM, + .max_regen_torque = PhysicalParameters::AMK_MAX_TORQUE, + .max_torque = PhysicalParameters::AMK_MAX_TORQUE, }; CASESystem case_system(&faked_msg_q, 100, 70, 550, case_config); } diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h deleted file mode 100644 index 64a63b5f0..000000000 --- a/test/test_systems/torque_controller_mux_test.h +++ /dev/null @@ -1,811 +0,0 @@ -#ifndef TORQUE_CONTROLLER_MUX_TEST -#define TORQUE_CONTROLLER_MUX_TEST - -#include -#include -#include -#include -#include -#include - - -#include "AnalogSensorsInterface.h" -#include "DashboardInterface.h" -#include "PhysicalParameters.h" -#include "TorqueControllers.h" -#include "VectornavInterface.h" - -// TODO: Find out why this test intermittently fails -// TEST(TorqueControllerMuxTesting, test_torque_button) -// { -// const int SERIES_LENGTH = 10; -// SysClock clock = SysClock(); -// SysTick_s cur_tick; -// TorqueControllerMux torque_controller_mux = TorqueControllerMux(); - -// bool buttonTimeSeries[SERIES_LENGTH]; // 50hz time series -// // Cycle back to low test -// buttonTimeSeries[1] = true; -// buttonTimeSeries[3] = true; -// buttonTimeSeries[5] = true; -// // Cycle only on button release test -// for (int i = 7; i < SERIES_LENGTH; i++) -// buttonTimeSeries[i] = true; - -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); - -// // Run test -// for (int i = 0; i < SERIES_LENGTH; i++) -// { -// cur_tick = clock.tick(i * 1000 * 20); // 20 ms increments -// torque_controller_mux.tick( -// cur_tick, -// (const DrivetrainDynamicReport_s) {}, -// (const PedalsSystemData_s) {}, -// (const SteeringSystemData_s) {}, -// (const AnalogConversion_s) {}, -// (const AnalogConversion_s) {}, -// (const AnalogConversion_s) {}, -// (const AnalogConversion_s) {}, -// DialMode_e::MODE_1, -// buttonTimeSeries[i] -// ); -// // Test conditions -// if (i == 2) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_MID_TORQUE); -// if (i == 4) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_FULL_TORQUE); -// if (i == 6) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); -// if (i == SERIES_LENGTH - 1) -// ASSERT_EQ(torque_controller_mux.getTorqueLimit(), TorqueLimit_e::TCMUX_LOW_TORQUE); -// } -// } - -TEST(TorqueControllerMuxTesting, test_torque_delta_prevents_mode_change) -{ - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TelemetryInterface telem_interface; - TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s resulting_torque_command; - - vector_nav vn_data{}; - - DrivetrainDynamicReport_s simulated_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0.0, 0.0, 0.0, 0.0}, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - PedalsSystemData_s simulated_full_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_no_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 0.0, - .brakePercent = 0.0 - }; - - // Press the pedal and observe whether torque mode changes - // Torque mode should not change since the requested torque is vastly different from the currently requested torque (0) - torque_controller_mux.tick( - cur_tick, - simulated_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - 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 - torque_controller_mux.tick( - cur_tick, - simulated_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // Now press the pedal again. Torque should be requested - torque_controller_mux.tick( - cur_tick, - simulated_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_GT(resulting_torque_command.torqueSetpoints[i], 5.0); - ASSERT_GT(resulting_torque_command.speeds_rpm[i], 5.0); - } -} - -TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) -{ - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TelemetryInterface telem_interface; - TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s resulting_torque_command; - vector_nav vn_data{}; - - DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - PedalsSystemData_s simulated_full_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_no_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 0.0, - .brakePercent = 0.0 - }; - - // Pretend the drivetrain is moving above MAX_SPEED_FOR_MODE_CHANGE and attempt mode shift (TC_NO_CONTROLLER to TC_SAFE_MODE) via tick - // Torque mode should not change since the car is moving - torque_controller_mux.tick( - cur_tick, - simulated_fast_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_LE(resulting_torque_command.torqueSetpoints[i], 0.0001); - ASSERT_LE(resulting_torque_command.speeds_rpm[i], 0.0001); - } - - // Tell TCMUX vehicle is stationary. Mode should change - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // Now press the pedal. Torque should be requested - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - resulting_torque_command = torque_controller_mux.getDrivetrainCommand(); - - for (int i = 0; i < NUM_MOTORS; i++) - { - ASSERT_GT(resulting_torque_command.torqueSetpoints[i], 5.0); - ASSERT_GT(resulting_torque_command.speeds_rpm[i], 5.0); - } -} - -TEST(TorqueControllerMuxTesting, test_power_limit) { - TelemetryInterface telem_interface; - TorqueControllerMux mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s drive_command; - - DrivetrainDynamicReport_s edit; - - for (int i = 0; i < 4; i++) { - edit.measuredSpeeds[i] = 500.0f; - drive_command.torqueSetpoints[i] = 10.0f; - } - - const DrivetrainDynamicReport_s drivetrain1 = edit; - - mux.applyPowerLimit(&drive_command, &drivetrain1); - - for (int i = 0; i < 4; i++) { - ASSERT_EQ(drive_command.torqueSetpoints[i], 10.0f); - } - - for (int i = 0; i < 4; i++) { - edit.measuredSpeeds[i] = 20000.0f; - drive_command.torqueSetpoints[i] = 21.0f; - } - - const DrivetrainDynamicReport_s drivetrain2 = edit; - - mux.applyPowerLimit(&drive_command, &drivetrain2); - - for (int i = 0; i < 4; i++) { - ASSERT_LT(drive_command.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator - } - -} - -TEST(TorqueControllerMuxTesting, test_torque_limit) { - TelemetryInterface telem_interface; - TorqueControllerMux mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s drive_command; - - for (int i = 0; i < 4; i++) { - drive_command.speeds_rpm[i] = 500.0f; - drive_command.torqueSetpoints[i] = 10.0f; - } - - drive_command.torqueSetpoints[0] = 5; - - mux.applyTorqueLimit(&drive_command); - - ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); - ASSERT_EQ(drive_command.torqueSetpoints[1], 10.0f); - ASSERT_EQ(drive_command.torqueSetpoints[2], 10.0f); - ASSERT_EQ(drive_command.torqueSetpoints[3], 10.0f); - - for (int i = 0; i < 4; i++) { - drive_command.speeds_rpm[i] = 500.0f; - drive_command.torqueSetpoints[i] = 20.0f; - } - drive_command.torqueSetpoints[0] = 5; - - mux.applyTorqueLimit(&drive_command); - - ASSERT_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]); - printf("torque 3: %.2f\n", drive_command.torqueSetpoints[2]); - printf("torque 4: %.2f\n", drive_command.torqueSetpoints[3]); -} - -TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TelemetryInterface telem_interface; - TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s resulting_torque_command; - - vector_nav vn_data{}; - - DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_barely_launch_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0, 2785.86, 0, 0}, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_no_launch_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0, 2786.86, 0, 0}, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - PedalsSystemData_s simulated_full_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_no_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 0.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_accel_and_brake_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.3 - }; - - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // change mode to mode 3 - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick again to calculate state switch - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); - - // press accelerator but with one wheel right at max speed threshold - torque_controller_mux.tick( - cur_tick, - simulated_no_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_NOT_READY); - - // go back to launch ready state - torque_controller_mux.tick( - cur_tick, - simulated_barely_launch_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); - - // press accel now with one wheelspeed barely under threshold - torque_controller_mux.tick( - cur_tick, - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - torque_controller_mux.tick( - clock.tick(1000000), // 1 second since launch - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); - - // this fails, but the value is close enough - // ASSERT_EQ((float)((int)((commanded.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f); - - torque_controller_mux.tick( - clock.tick(1000000), // 1 second since launch - simulated_barely_launch_drivetrain_dynamics, - simulated_accel_and_brake_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_3, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_NOT_READY); -} - -TEST(TorqueControllerMuxTesting, test_slip_launch_controller) { - SysClock clock = SysClock(); - SysTick_s cur_tick; - cur_tick = clock.tick(0); - TelemetryInterface telem_interface; - TorqueControllerMux torque_controller_mux = TorqueControllerMux(&telem_interface); - DrivetrainCommand_s resulting_torque_command; - - vector_nav vn_data{}; - - DrivetrainDynamicReport_s simulated_slow_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = { - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM), - ((MAX_SPEED_FOR_MODE_CHANGE - 0.1f) * METERS_PER_SECOND_TO_RPM) - }, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - DrivetrainDynamicReport_s simulated_barely_launch_drivetrain_dynamics = { - .measuredInverterFLPackVoltage = 550, - .measuredSpeeds = {0, 2785.86, 0, 0}, - .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - }; - - // DrivetrainDynamicReport_s simulated_no_launch_drivetrain_dynamics = { - // .measuredInverterFLPackVoltage = 550, - // .measuredSpeeds = {0, 2786.86, 0, 0}, - // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - // }; - - // DrivetrainDynamicReport_s simulated_fast_drivetrain_dynamics = { - // .measuredInverterFLPackVoltage = 550, - // .measuredSpeeds = { - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM), - // ((MAX_SPEED_FOR_MODE_CHANGE + 0.1f) * METERS_PER_SECOND_TO_RPM) - // }, - // .measuredTorques = {0.0, 0.0, 0.0, 0.0}, - // .measuredTorqueCurrents = {0.0, 0.0, 0.0, 0.0}, - // .measuredMagnetizingCurrents = {0.0, 0.0, 0.0, 0.0} - // }; - - PedalsSystemData_s simulated_full_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 1.0, - .brakePercent = 0.0 - }; - - PedalsSystemData_s simulated_no_accel_press = { - .accelImplausible = false, - .brakeImplausible = false, - .brakePressed = false, - .brakeAndAccelPressedImplausibility = false, - .implausibilityExceededMaxDuration = false, - .accelPercent = 0.0, - .brakePercent = 0.0 - }; - - // PedalsSystemData_s simulated_accel_and_brake_press = { - // .accelImplausible = false, - // .brakeImplausible = false, - // .brakePressed = false, - // .brakeAndAccelPressedImplausibility = false, - // .implausibilityExceededMaxDuration = false, - // .accelPercent = 1.0, - // .brakePercent = 0.3 - // }; - - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_0, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // change mode to mode 3 - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick again to calculate state switch - torque_controller_mux.tick( - cur_tick, - simulated_slow_drivetrain_dynamics, - simulated_no_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - LaunchStates_e launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCH_READY); - - // press accel now with one wheelspeed barely under threshold - torque_controller_mux.tick( - clock.tick(100000), - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - // tick TCMUX (10k us will hit 100hz) - torque_controller_mux.tick( - clock.tick(100000), - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - DrivetrainCommand_s commanded = torque_controller_mux.getDrivetrainCommand(); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); - - torque_controller_mux.tick( - clock.tick(const_accel_time * 1000), // const accel time sine launch - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - launch_state = torque_controller_mux.activeController()->get_launch_state(); - - ASSERT_EQ(launch_state, LaunchStates_e::LAUNCHING); - - commanded = torque_controller_mux.getDrivetrainCommand(); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0], DEFAULT_LAUNCH_SPEED_TARGET); - - // if velocity is less than the default speed, it should still go at launch speed - vn_data.velocity_x = 0; // m/s - - torque_controller_mux.tick( - clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - commanded = torque_controller_mux.getDrivetrainCommand(); - printf("lower vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); - ASSERT_EQ(commanded.speeds_rpm[0] , DEFAULT_LAUNCH_SPEED_TARGET); // should still be at initial launch target - - // this is approx. the speed the car would be going at 1500 rpm - // we should expect the next calculation to be approx. DEFAULT_SLIP_RATIO % higher than this - vn_data.velocity_x = DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND; // m/s - - torque_controller_mux.tick( - clock.tick((const_accel_time * 1000) + 1000000), // 1 second after const accel time - simulated_barely_launch_drivetrain_dynamics, - simulated_full_accel_press, - (const SteeringSystemData_s) {}, - (const LoadCellInterfaceOutput_s) {}, - DialMode_e::MODE_4, - 1.0, - false, - vn_data, - (const DrivetrainCommand_s) {} - ); - - commanded = torque_controller_mux.getDrivetrainCommand(); - printf("launch vx_body: %.2f\n", (float)commanded.speeds_rpm[0] * RPM_TO_METERS_PER_SECOND); - ASSERT_EQ((int16_t)commanded.speeds_rpm[0] > (DEFAULT_LAUNCH_SPEED_TARGET * RPM_TO_METERS_PER_SECOND), true); - - - // this fails, but the value is close enough - // ASSERT_EQ((float)((int)((commanded.speeds_rpm[0] - 1500) * RPM_TO_METERS_PER_SECOND * 100)) / 100, 11.76f); -} - -#endif /* TORQUE_CONTROLLER_MUX_TEST */ \ No newline at end of file