From ee32c25039817b679921702d756947c023920b83 Mon Sep 17 00:00:00 2001 From: CL16gtgh Date: Fri, 24 May 2024 23:03:52 -0400 Subject: [PATCH] Squashed commit of the following: commit 6bd7ba44c7be5b74955c8c6164732531eec9a3f1 Merge: 2beeda82 18152729 Author: CL16gtgh Date: Fri May 24 21:35:01 2024 -0400 Merge branch 'master' into feature/TCMux-relevant-status-tracking commit 2beeda82da902a27599f6a2109848db0283daa60 Author: CL16gtgh Date: Wed May 22 18:55:23 2024 -0400 Trying to get rid of self limiting features commit 65f9ba00df78313c1c7a56c8ef8faea73a36322c Author: CL16gtgh Date: Wed May 22 17:36:43 2024 -0400 Squashed commit of the following: commit 0b688704841d2fa14da35a5fa5159061fa30a375 Author: CL16gtgh Date: Wed May 22 17:33:24 2024 -0400 Squashed commit of the following: commit 07faefaba316f8a090ab1d0191dd06c95f139e82 Merge: f149cdcc bb038370 Author: CL16gtgh Date: Wed May 22 17:29:16 2024 -0400 Merge branch 'master' into CASE_testbranch commit f149cdcc7d914b543d060fb815c316e24461bb95 Author: CL16gtgh Date: Wed May 22 17:06:46 2024 -0400 Reapply "bad" This reverts commit 2a253cde26abbe9648e8cf12feb2ffedcb2c630d. commit e8cbee792d0dbe17f318fcc82e0d5b3bef443b1d Author: CL16gtgh Date: Wed May 22 17:06:35 2024 -0400 Reapply "Update main.cpp" This reverts commit 50de74cb8fdf54dfb73dd047d41d2618a97a3503. commit 3d1aab350837aa61484f96b4e33bf0752168c37b Author: CL16gtgh Date: Wed May 22 17:06:23 2024 -0400 Reapply "Merge branch 'master' into CASE_testbranch" This reverts commit e062970d4409bd761214e392f27842b7e6870ec2. commit e062970d4409bd761214e392f27842b7e6870ec2 Author: CL16gtgh Date: Wed May 22 16:37:16 2024 -0400 Revert "Merge branch 'master' into CASE_testbranch" This reverts commit df74595acc64ee569cfd9377f0f344c784be3323. commit 50de74cb8fdf54dfb73dd047d41d2618a97a3503 Author: CL16gtgh Date: Wed May 22 16:37:07 2024 -0400 Revert "Update main.cpp" This reverts commit dfa4f7a3cf177dcaee65c59cd3a406c253359288. commit 2a253cde26abbe9648e8cf12feb2ffedcb2c630d Author: CL16gtgh Date: Wed May 22 16:36:55 2024 -0400 Revert "bad" This reverts commit 95c4618988317737ebc4a31afa6c6d5ee3a2e27b. commit 95c4618988317737ebc4a31afa6c6d5ee3a2e27b Author: CL16gtgh Date: Wed May 22 16:36:42 2024 -0400 bad commit dfa4f7a3cf177dcaee65c59cd3a406c253359288 Author: CL16gtgh Date: Wed May 22 16:24:22 2024 -0400 Update main.cpp commit df74595acc64ee569cfd9377f0f344c784be3323 Author: CL16gtgh Date: Wed May 22 16:23:20 2024 -0400 Merge branch 'master' into CASE_testbranch commit bb038370a0f0c13cd0e1e4bcb25e634b6ee7feed Author: CL16gtgh Date: Wed May 22 15:10:02 2024 -0400 more detailed steering report commit 2b5642365faf8db118b26bba7cd2324b8d8ffa7f Author: CL16gtgh Date: Wed May 22 14:26:14 2024 -0400 not working when steered to right commit 40dd816373d34d825800ed9cf16a17fd0994ee7d Author: CL16gtgh Date: Tue May 21 18:05:55 2024 -0400 Revert "Added Derate Factor code to AMS interface" This reverts commit 5262fe863b9df002e12565476522d49171ff36b1. commit 760bd8f0acf3683092601822cb4682c4eb286568 Author: CL16gtgh Date: Tue May 21 18:04:56 2024 -0400 Revert "added derate factor to case and loadcell" This reverts commit 5d9761b97bdf00b7dc1c1b5c5e89d01f0a533253. commit 5d9761b97bdf00b7dc1c1b5c5e89d01f0a533253 Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Tue May 21 00:07:20 2024 -0700 added derate factor to case and loadcell commit 5262fe863b9df002e12565476522d49171ff36b1 Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Mon May 20 23:53:20 2024 -0700 Added Derate Factor code to AMS interface commit 23be5cd4e88243c9d4273c0ad2025a7270980f4c Merge: 8ac101b5 1416d3eb Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Mon May 20 22:54:56 2024 -0700 Merge branch 'master' of https://github.com/hytech-racing/MCU commit 1416d3eb16d117c7cb9bd9fb9273aa1d3a93779c Author: Ben Hall Date: Sat May 11 15:24:45 2024 -0400 adding private member functions to doxygen docs commit 408193b6b7111dbdc48f6af84ce1b50195d7a855 Author: Ben Hall Date: Sat May 11 15:11:22 2024 -0400 fixing repo library dep clone directives commit 28d5e7226319c65f8976b519d1162793991da576 Author: CL16gtgh Date: Wed May 22 16:17:14 2024 -0400 Revert "Squashed commit of the following:" This reverts commit 25136c7f3b5790c11e6ad2022c372ac16ea1a189. commit 25136c7f3b5790c11e6ad2022c372ac16ea1a189 Author: CL16gtgh Date: Wed May 22 16:15:59 2024 -0400 Squashed commit of the following: commit bb038370a0f0c13cd0e1e4bcb25e634b6ee7feed Author: CL16gtgh Date: Wed May 22 15:10:02 2024 -0400 more detailed steering report commit 2b5642365faf8db118b26bba7cd2324b8d8ffa7f Author: CL16gtgh Date: Wed May 22 14:26:14 2024 -0400 not working when steered to right commit 23be5cd4e88243c9d4273c0ad2025a7270980f4c Merge: 8ac101b5 1416d3eb Author: shaynoorani <113149316+shaynoorani@users.noreply.github.com> Date: Mon May 20 22:54:56 2024 -0700 Merge branch 'master' of https://github.com/hytech-racing/MCU commit 1416d3eb16d117c7cb9bd9fb9273aa1d3a93779c Author: Ben Hall Date: Sat May 11 15:24:45 2024 -0400 adding private member functions to doxygen docs commit 408193b6b7111dbdc48f6af84ce1b50195d7a855 Author: Ben Hall Date: Sat May 11 15:11:22 2024 -0400 fixing repo library dep clone directives commit ccd205a147946ce2619be9fbd60d3a7be67dfcd3 Author: CL16gtgh Date: Wed May 22 15:50:25 2024 -0400 finale commit d0c523e211f2316917893533fb185f0d1f8f3ee2 Author: CL16gtgh Date: Wed May 22 14:57:58 2024 -0400 read more values commit b528dfc3c72147cf90c0177a1c478504a2fd2db8 Merge: f4e69efc 207b2991 Author: CL16gtgh Date: Mon May 20 21:26:55 2024 -0400 Merge branch 'CASE_testbranch' into feature/filter-steering-reading commit 207b29915f61fa55f6a80a9fe54bd92001e9207a Author: Luke Chen <43226681+Luke-kC@users.noreply.github.com> Date: Sat May 18 00:59:13 2024 -0700 Decoupled and NL scheduled slip targets for TCS commit afda31e97927c150a94b44c0c8617ebd34a851a6 Author: CL16gtgh Date: Wed May 22 16:00:49 2024 -0400 Squashed commit of the following: commit ccd205a147946ce2619be9fbd60d3a7be67dfcd3 Author: CL16gtgh Date: Wed May 22 15:50:25 2024 -0400 finale commit d0c523e211f2316917893533fb185f0d1f8f3ee2 Author: CL16gtgh Date: Wed May 22 14:57:58 2024 -0400 read more values commit b528dfc3c72147cf90c0177a1c478504a2fd2db8 Merge: f4e69efc 207b2991 Author: CL16gtgh Date: Mon May 20 21:26:55 2024 -0400 Merge branch 'CASE_testbranch' into feature/filter-steering-reading commit 207b29915f61fa55f6a80a9fe54bd92001e9207a Author: Luke Chen <43226681+Luke-kC@users.noreply.github.com> Date: Sat May 18 00:59:13 2024 -0700 Decoupled and NL scheduled slip targets for TCS commit b73616916b859496200d85c5c513a4e5390b70ea Author: CL16gtgh Date: Wed May 22 15:52:14 2024 -0400 Fix skill issue commit 9facf9ebb1ccbab60920bd45ba3f6566d1b14bfc Merge: 3daa65bc f4e69efc Author: CL16gtgh Date: Mon May 20 01:19:17 2024 -0400 Merge branch 'feature/filter-steering-reading' into feature/TCMux-relevant-status-tracking commit 3daa65bcdb64796f2ae48b1196cf0cc25fb5171a Author: CL16gtgh Date: Mon May 20 00:11:51 2024 -0400 a lil bit of ocd commit 1e5326a37c199fba7a919d5c55c3b2b6745e579f Author: CL16gtgh Date: Sun May 19 23:20:57 2024 -0400 Added implementation in TelemetryInterface and updated .ini file commit 3a1f493e97938ab68cec98103d8e9df7e637a6ee Author: CL16gtgh Date: Sun May 19 18:40:13 2024 -0400 Added steering status report commit 1284ebc4bd482d4f5976c2c0220309b87e7ebcd6 Author: CL16gtgh Date: Sun May 19 16:02:14 2024 -0400 Idea outline for reporting TCMux status --- include/MCU_rev15_defs.h | 4 ++ lib/interfaces/include/TelemetryInterface.h | 44 +++++++++----- lib/interfaces/src/TelemetryInterface.cpp | 41 ++++++++++++- .../include/TorqueControllersData.h | 23 ++++++- lib/systems/include/SteeringSystem.h | 23 +++++-- lib/systems/include/TorqueControllerMux.h | 52 +++++++++------- lib/systems/include/TorqueControllers.h | 1 - lib/systems/src/SteeringSystem.cpp | 35 +++++++++-- lib/systems/src/TorqueControllerMux.cpp | 60 ++++++++++++++++--- platformio.ini | 2 +- src/main.cpp | 4 +- 11 files changed, 224 insertions(+), 65 deletions(-) diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 1ca6f11fb..84353bdc3 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -105,4 +105,8 @@ const int SECONDARY_STEERING_SENSE_CENTER = 1945; // 1960 // 1970 const float STEERING_RANGE_DEGREES = 257.0f; // 253.0f // 256.05f // 134+130-7(slop) const float STEERING_IIR_ALPHA = 0.7f; // shaves off around 1 deg of max discrepancy +// TC parameters +const float SIMPLE_TC_REAR_TORQUE_SCALE = 1.0; +const float SIMPLE_TC_REGEN_TORQUE_SCALE = 0.4; + #endif /* __MCU15_H__ */ \ No newline at end of file diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index c94223e3e..2e5226183 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -47,7 +47,7 @@ class TelemetryInterface TelemetryInterface(CANBufferType *msg_output_queue, const TelemetryInterfaceReadChannels &channels) : msg_queue_(msg_output_queue), channels_(channels){}; - /* GETTERS for basic conversions that don't have a home*/ + /* GETTERS for basic conversions that don't have a home */ AnalogConversion_s get_glv_voltage(const AnalogConversionPacket_s<8> &adc1) { @@ -75,23 +75,27 @@ class TelemetryInterface const AnalogConversion_s &lc_fl, const AnalogConversion_s &lc_fr, const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr); + const AnalogConversion_s &pots_fr + ); void update_analog_readings_CAN_msg( const SteeringEncoderConversion_s &steer1, const AnalogConversion_s &steer2, const AnalogConversion_s ¤t, const AnalogConversion_s &reference, - const AnalogConversion_s &glv); + const AnalogConversion_s &glv + ); void update_drivetrain_rpms_CAN_msg( InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); void update_drivetrain_err_status_CAN_msg( InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); void update_drivetrain_status_telem_CAN_msg( InvInt_t *fl, InvInt_t *fr, @@ -100,31 +104,39 @@ class TelemetryInterface bool accel_implaus, bool brake_implaus, float accel_per, - float brake_per); + float brake_per + ); void update_drivetrain_torque_telem_CAN_msg( InvInt_t *fl, InvInt_t *fr, InvInt_t *rl, - InvInt_t *rr); + InvInt_t *rr + ); void update_penthouse_accum_CAN_msg( const AnalogConversion_s ¤t, - const AnalogConversion_s &reference); - - void enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data); + const AnalogConversion_s &reference + ); + void update_TCMux_status_CAN_msg( + const TCMuxStatus_s &tcMuxStatus + ); + void update_steering_status_CAN_msg( + const float steering_system_angle, + const float filtered_angle_encoder, + const float filtered_angle_analog, + const uint8_t steering_system_status, + const uint8_t steering_encoder_status, + const uint8_t steering_analog_status + ); /* Enqueue outbound telemetry CAN messages */ - // void enqueue_CAN_mcu_pedal_readings(); - // void enqueue_CAN_mcu_load_cells(); - // void enqueue_CAN_mcu_front_potentiometers(); - // void enqueue_CAN_mcu_rear_potentiometers(); - // void enqueue_CAN_mcu_analog_readings(); - template void enqueue_CAN(T can_msg, uint32_t id); template void enqueue_new_CAN(U *structure, uint32_t (*pack_function)(U *, uint8_t *, uint8_t *, uint8_t *)); + void enqeue_controller_CAN_msg(const PIDTVTorqueControllerData &data); + // TODO dont pass in pointer to inverter interface here we will break shit with this /* Tick at 50Hz to send CAN */ void tick( diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index 963a83740..c8b6ede89 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -61,8 +61,8 @@ void TelemetryInterface::update_front_thermistors_CAN_msg(const AnalogConversion const AnalogConversion_s &therm_fr) { FRONT_THERMISTORS_t front_thermistors_; - front_thermistors_.thermistor_motor_fl = therm_fl.raw; - front_thermistors_.thermistor_motor_fr = therm_fr.raw; + front_thermistors_.thermistor_motor_fl_ro = HYTECH_thermistor_motor_fl_ro_toS(therm_fl.raw); + front_thermistors_.thermistor_motor_fr_ro = HYTECH_thermistor_motor_fr_ro_toS(therm_fr.raw); enqueue_new_CAN(&front_thermistors_, &Pack_FRONT_THERMISTORS_hytech); } @@ -194,6 +194,42 @@ void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s enqueue_new_CAN(&message, &Pack_PENTHOUSE_ACCUM_MSG_hytech); } +void TelemetryInterface::update_TCMux_status_CAN_msg(const TCMuxStatus_s &tcMuxStatus) +{ + TCMUX_STATUS_REPORT_t msg; + + msg.speed_above_thresh = tcMuxStatus.speedPreventsModeChange; + msg.torque_delta_above_thresh = tcMuxStatus.torqueDeltaPreventsModeChange; + msg.tc_not_ready = tcMuxStatus.controllerNotReadyPreventsModeChange; + msg.steering_system_has_err = tcMuxStatus.steeringSystemError; + msg.mode_intended = tcMuxStatus.modeIntended; + msg.mode_actual = tcMuxStatus.modeActual; + msg.dash_dial_mode = tcMuxStatus.dialMode; + msg.torque_mode = tcMuxStatus.torqueMode; + msg.torque_limit_ro = HYTECH_torque_limit_ro_toS(tcMuxStatus.maxTorque); + + enqueue_new_CAN(&msg, &Pack_TCMUX_STATUS_REPORT_hytech); +} + +void TelemetryInterface::update_steering_status_CAN_msg(const float steering_system_angle, + const float filtered_angle_encoder, + const float filtered_angle_analog, + const uint8_t steering_system_status, + const uint8_t steering_encoder_status, + const uint8_t steering_analog_status) +{ + STEERING_SYSTEM_REPORT_t msg; + + msg.steering_system_angle_ro = HYTECH_steering_system_angle_ro_toS(steering_system_angle); + msg.steering_encoder_angle_ro = HYTECH_steering_encoder_angle_ro_toS(filtered_angle_encoder); + msg.steering_analog_angle_ro = HYTECH_steering_analog_angle_ro_toS(filtered_angle_analog); + msg.steering_system_status = steering_system_status; + msg.steering_encoder_status = steering_encoder_status; + msg.steering_analog_status = steering_analog_status; + + enqueue_new_CAN(&msg, &Pack_STEERING_SYSTEM_REPORT_hytech); +} + /* Send CAN messages */ template void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { @@ -237,7 +273,6 @@ void TelemetryInterface::enqeue_controller_CAN_msg(const PIDTVTorqueControllerDa } - /* Tick SysClock */ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, diff --git a/lib/shared_data/include/TorqueControllersData.h b/lib/shared_data/include/TorqueControllersData.h index f54d510a3..c35a0cb51 100644 --- a/lib/shared_data/include/TorqueControllersData.h +++ b/lib/shared_data/include/TorqueControllersData.h @@ -1,5 +1,22 @@ -#ifndef TORQUECONTROLLERSDATA -#define TORQUECONTROLLERSDATA +#ifndef __TORQUE_CONTROLLERS_DATA__ +#define __TORQUE_CONTROLLERS_DATA__ + +#include + +struct TCMuxStatus_s +{ + bool speedPreventsModeChange; + bool torqueDeltaPreventsModeChange; + bool controllerNotReadyPreventsModeChange; + bool steeringSystemError; + + uint8_t modeIntended; + uint8_t modeActual; + uint8_t dialMode; + + uint8_t torqueMode; + float maxTorque; +}; struct PIDTVTorqueControllerData { @@ -11,4 +28,4 @@ struct PIDTVTorqueControllerData float rr_torque_delta; }; -#endif \ No newline at end of file +#endif /* __TORQUE_CONTROLLERS_DATA__ */ \ No newline at end of file diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index c2fa7f7c5..6810541bb 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/lib/systems/include/SteeringSystem.h @@ -5,6 +5,8 @@ #include "AnalogSensorsInterface.h" #include "Filter_IIR.h" #include "SysClock.h" +#include "TelemetryInterface.h" + // Digital Encoder = Primary Sensor // Analog Encoder = Secondary Sensor @@ -52,17 +54,20 @@ class SteeringSystem Filter_IIR steeringFilters_[NUM_SENSORS]; float filteredAnglePrimary_; float filteredAngleSecondary_; + + TelemetryInterface *telemHandle_; public: - SteeringSystem(SteeringEncoderInterface *primarySensor) - : SteeringSystem(primarySensor, DEFAULT_STEERING_ALPHA) + SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface) + : SteeringSystem(primarySensor, telemInterface, DEFAULT_STEERING_ALPHA) {} - SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlpha) - : SteeringSystem(primarySensor, filterAlpha, filterAlpha) + SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface, float filterAlpha) + : SteeringSystem(primarySensor, telemInterface, filterAlpha, filterAlpha) {} - SteeringSystem(SteeringEncoderInterface *primarySensor, float filterAlphaPrimary, float filterAlphaSecondary) + SteeringSystem(SteeringEncoderInterface *primarySensor, TelemetryInterface *telemInterface, float filterAlphaPrimary, float filterAlphaSecondary) : primarySensor_(primarySensor) + , telemHandle_(telemInterface) { steeringFilters_[0] = Filter_IIR(filterAlphaPrimary); steeringFilters_[1] = Filter_IIR(filterAlphaSecondary); @@ -80,6 +85,14 @@ class SteeringSystem { return steeringData_; } + + void reportSteeringStatus( + const float angle, + const float filteredAngleEncoder, + const float filteredAngleAnalog, + const uint8_t systemStatus, + const uint8_t encoderStatus, + const uint8_t analogSensorStatus); }; #endif /* __STEERINGSYSTEM_H__ */ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index cd8dc0501..972c2cce8 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -11,11 +11,12 @@ #include "DashboardInterface.h" #include "VectornavInterface.h" #include "LoadCellInterface.h" +#include "TelemetryInterface.h" const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm -/// @brief +/// @brief multiplexer class for managing available torque controllers class TorqueControllerMux { private: @@ -42,11 +43,11 @@ class TorqueControllerMux }; TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; - DialMode_e cur_dial_mode_ = DialMode_e::MODE_0; + DialMode_e currDialMode_ = DialMode_e::MODE_0; TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; - + // Handle array for all torque controllers TorqueControllerBase* controllers[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)] = { static_cast(&torqueControllerNone_), static_cast(&torqueControllerSimple_), @@ -56,33 +57,41 @@ class TorqueControllerMux static_cast(&tcCASEWrapper_) }; + // Status tracking structure for visibility + TCMuxStatus_s tcMuxStatus_; + DrivetrainCommand_s drivetrainCommand_; - TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE; + TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_FULL_TORQUE; bool torqueLimitButtonPressed_ = false; unsigned long torqueLimitButtonPressedTime_ = 0; + TelemetryInterface *telemHandle_; public: /// @brief torque controller mux in which default instances of all torque controllers are created for use - TorqueControllerMux() + TorqueControllerMux(TelemetryInterface *telemInterface) : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)]) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) - , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} + , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) + , telemHandle_(telemInterface) {} /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC /// @param simpleTCRearTorqueScale the scaling from 0 to 2 in which 2 is full rear torque allocation, 0 is full front, 1 = balanced /// @param simpleTCRegenTorqueScale scaling from 0 to 2 in which 0 is full rear regen and 2 is full front regen, 1 = balanced - TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale) + TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale, TelemetryInterface *telemInterface) : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) - , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) {} -// Functions + , tcCASEWrapper_(controllerOutputs_[static_cast(TorqueController_e::TC_CASE_SYSTEM)]) + , telemHandle_(telemInterface) {} + + // Functions + /// @brief tick controllers to calculate drivetrain command void tick( const SysTick_s &tick, const DrivetrainDynamicReport_s &drivetrainData, @@ -94,12 +103,20 @@ class TorqueControllerMux 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); + + /// @brief GETTERS const DrivetrainCommand_s &getDrivetrainCommand() { return drivetrainCommand_; }; - const TorqueLimit_e &getTorqueLimit() + const TorqueLimit_e getTorqueLimit() { return torqueLimit_; }; @@ -109,19 +126,9 @@ class TorqueControllerMux return torqueLimitMap_[torqueLimit_]; } - void applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); - - void applyTorqueLimit(DrivetrainCommand_s* command); - - void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); - - void applyPosSpeedLimit(DrivetrainCommand_s* command); - - - const DialMode_e getDialMode() { - return cur_dial_mode_; + return currDialMode_; } const TorqueController_e getDriveMode() @@ -139,6 +146,9 @@ class TorqueControllerMux return static_cast(&torqueControllerNone_); } } + + /// @brief report TCMux status through Telemetry via CAN + void reportTCMuxStatus(); }; #endif /* __TORQUECTRLMUX_H__ */ diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 18ed91e28..e0bc89ada 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -4,7 +4,6 @@ #include #include #include -#include #include "AnalogSensorsInterface.h" #include "DashboardInterface.h" #include "PhysicalParameters.h" diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index 32439736e..fb647937f 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -61,11 +61,36 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR }; } + + } - // For possible bottom sensor recalibration - // TODO: Remove me once done! - // Serial.print("Secondary sensor raw: "); - // Serial.println(intake.secondaryConversion.raw); - // Serial.println(); + // Report at 50Hz + if (intake.tick.triggers.trigger50) + { + reportSteeringStatus( + steeringData_.angle, + filteredAnglePrimary_, + filteredAngleSecondary_, + static_cast(steeringData_.status), + static_cast(primaryConversion_.status), + static_cast(intake.secondaryConversion.status)); } + + +} + +void SteeringSystem::reportSteeringStatus(const float angle, + const float filteredAngleEncoder, + const float filteredAngleAnalog, + const uint8_t systemStatus, + const uint8_t encoderStatus, + const uint8_t analogSensorStatus) +{ + telemHandle_->update_steering_status_CAN_msg( + angle, + filteredAngleEncoder, + filteredAngleAnalog, + systemStatus, + encoderStatus, + analogSensorStatus); } diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 96b8b705f..6aaa4bcda 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -27,8 +27,7 @@ void TorqueControllerMux::tick( if (tick.triggers.trigger50) { // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms - if ( - torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) + if (torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) { // WOW C++ is ass torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); @@ -74,8 +73,13 @@ void TorqueControllerMux::tick( // if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) // { muxMode_ = dialModeMap_[dashboardDialMode]; - cur_dial_mode_ = dashboardDialMode; + currDialMode_ = dashboardDialMode; // } + + // Update TCMux status + tcMuxStatus_.speedPreventsModeChange = speedPreventsModeChange; + tcMuxStatus_.torqueDeltaPreventsModeChange = torqueDeltaPreventsModeChange; + tcMuxStatus_.controllerNotReadyPreventsModeChange = controllerNotReadyPreventsModeChange; } // Check if the current controller is ready. If it has faulted, revert to safe mode @@ -86,17 +90,40 @@ void TorqueControllerMux::tick( muxMode_ = TorqueController_e::TC_SAFE_MODE; } + // Update TCMux status + tcMuxStatus_.steeringSystemError = steeringData.status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR; + tcMuxStatus_.modeIntended = static_cast(dialModeMap_[dashboardDialMode]); + tcMuxStatus_.modeActual = static_cast(getDriveMode()); + tcMuxStatus_.dialMode = static_cast(getDialMode()); + drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; + // Update TCMux status + tcMuxStatus_.torqueMode = static_cast(getTorqueLimit()); + tcMuxStatus_.maxTorque = getMaxTorque(); + // Apply setpoints value limits - // Safety checks for CASE: CASE handles regen, torque, and power limit internally - applyRegenLimit(&drivetrainCommand_, &drivetrainData); - // Apply torque limit before power limit to not power limit - applyTorqueLimit(&drivetrainCommand_); - applyPowerLimit(&drivetrainCommand_, &drivetrainData); + 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 + applyTorqueLimit(&drivetrainCommand_); + applyPowerLimit(&drivetrainCommand_, &drivetrainData); + } + // Uniformly apply speed limit to all controller modes applyPosSpeedLimit(&drivetrainCommand_); + + } + + // Update controller status report + if (tick.triggers.trigger50) + { + reportTCMuxStatus(); + } + } /* @@ -235,4 +262,21 @@ void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command) { command->speeds_rpm[i] = std::max(0.0f, command->speeds_rpm[i]); } +} + +/** + * Report TCMux status via CAN + * - speedPreventsModeChange + * - torqueDeltaPreventsModeChange + * - controllerNotReadyPreventsModeChange + * - steeringSystemError + * - modeIntended + * - modeActual + * - dialMode + * - torqueMode + * - maxTorque +*/ +void TorqueControllerMux::reportTCMuxStatus() +{ + telemHandle_->update_TCMux_status_CAN_msg(tcMuxStatus_); } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 5d9237b66..4567dd4f8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -55,7 +55,7 @@ lib_deps = https://github.com/RCMast3r/spi_libs#2214fee https://github.com/tonton81/FlexCAN_T4#b928bcb https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/97/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/99/can_lib.tar.gz git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 [env:test_can_on_teensy] diff --git a/src/main.cpp b/src/main.cpp index 8f2357000..80509074b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -142,7 +142,7 @@ struct inverters // */ SysClock sys_clock; -SteeringSystem steering_system(&steering1, STEERING_IIR_ALPHA); +SteeringSystem steering_system(&steering1, &telem_interface, STEERING_IIR_ALPHA); BuzzerController buzzer(BUZZER_ON_INTERVAL); SafetySystem safety_system(&ams_interface, &wd_interface); @@ -150,7 +150,7 @@ SafetySystem safety_system(&ams_interface, &wd_interface); PedalsSystem pedals_system(accel_params, brake_params); using DriveSys_t = DrivetrainSystem; DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); -TorqueControllerMux torque_controller_mux(1.0, 0.4); +TorqueControllerMux torque_controller_mux(SIMPLE_TC_REAR_TORQUE_SCALE, SIMPLE_TC_REGEN_TORQUE_SCALE, &telem_interface); // TODO ensure that case uses max regen torque, right now its not CASEConfiguration case_config = { // Following used for generated code