From 207b29915f61fa55f6a80a9fe54bd92001e9207a Mon Sep 17 00:00:00 2001 From: Luke Chen <43226681+Luke-kC@users.noreply.github.com> Date: Sat, 18 May 2024 00:59:13 -0700 Subject: [PATCH] Decoupled and NL scheduled slip targets for TCS --- lib/systems/include/CASESystem.h | 13 +++++++++++++ lib/systems/include/CASESystem.tpp | 30 ++++++++++++++++++++++-------- platformio.ini | 4 ++-- src/main.cpp | 17 ++++++++++++++--- 4 files changed, 51 insertions(+), 13 deletions(-) diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index 30449f33a..9207da8d5 100644 --- a/lib/systems/include/CASESystem.h +++ b/lib/systems/include/CASESystem.h @@ -70,6 +70,19 @@ struct CASEConfiguration float TCS_NL_endBoundPerc_FrontAxle; float TCS_NL_startBoundPerc_RearAxle; float TCS_NL_endBoundPerc_RearAxle; + bool useNL_TCS_SlipSchedule; + float launchSL_startBound_Front; + float launchSL_endBound_Front; + float launchSL_startBound_Rear; + float launchSL_endBound_Rear; + float TCS_SL_startBound_Front; + float TCS_SL_endBound_Front; + float TCS_SL_startBound_Rear; + float TCS_SL_endBound_Rear; + float TCS_SL_NLPerc_startBound_Front; + float TCS_SL_NLPerc_endBound_Front; + float TCS_SL_NLPerc_startBound_Rear; + float TCS_SL_NLPerc_endBound_Rear; float max_rpm; float max_regen_torque; diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 8c05abba8..38f914700 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -21,7 +21,9 @@ DrivetrainCommand_s CASESystem::evaluate( in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_regen_torque, config_.max_rpm); in.YawRaterads = vn_data.angular_rates.z; - // in.YawRaterads = 2.5; // THIS IS FAKE TODO HACK + + // FAKE + // in.YawRaterads = 2.5; // REAL in.Vx_B = vn_data.velocity_x; @@ -70,23 +72,26 @@ DrivetrainCommand_s CASESystem::evaluate( in.useTractionControl = config_.useTractionControl; - in.TCS_SLThreshold = config_.tcsSLThreshold; - in.LaunchSL = config_.launchSL; + in.TCS_SL_Targets[0] = config_.TCS_SL_startBound_Front; + in.TCS_SL_Targets[1] = config_.TCS_SL_endBound_Front; + in.TCS_SL_Targets[2] = config_.TCS_SL_startBound_Rear; + in.TCS_SL_Targets[3] = config_.TCS_SL_endBound_Rear; + + in.launchSL_Targets[0] = config_.launchSL_startBound_Front; + in.launchSL_Targets[1] = config_.launchSL_endBound_Front; + in.launchSL_Targets[2] = config_.launchSL_startBound_Rear; + in.launchSL_Targets[3] = config_.launchSL_endBound_Rear; + in.LaunchDeadZone = config_.launchDeadZone; in.TCSPIDConfig[0] = config_.tcs_pid_p_lowerBound_front; in.TCSPIDConfig[1] = config_.tcs_pid_p_upperBound_front; in.TCSPIDConfig[2] = config_.tcs_pid_p_lowerBound_rear; in.TCSPIDConfig[3] = config_.tcs_pid_p_upperBound_rear; - // in.TCSPIDConfig[2] = 0; - // in.TCSPIDConfig[3] = 0; in.TCSPIDConfig[4] = config_.tcs_pid_i; in.TCSPIDConfig[5] = config_.tcs_pid_d; - // in.TCSPIDConfig[4] = 50; - // in.TCSPIDConfig[5] = 50; in.LaunchVelThreshold = config_.launchVelThreshold; - in.TCSVelThreshold = config_.tcsVelThreshold; in.YawPIDErrorThreshold = config_.yawPIDErrorThreshold; in.YawPIDVelThreshold = config_.yawPIDVelThreshold; @@ -152,6 +157,13 @@ DrivetrainCommand_s CASESystem::evaluate( in.TCS_PID_NL_Schedule[2] = config_.TCS_NL_startBoundPerc_RearAxle; in.TCS_PID_NL_Schedule[3] = config_.TCS_NL_endBoundPerc_RearAxle; + in.TCS_SL_Targets_NLSchedule[0] = config_.TCS_SL_NLPerc_startBound_Front; + in.TCS_SL_Targets_NLSchedule[1] = config_.TCS_SL_NLPerc_endBound_Front; + in.TCS_SL_Targets_NLSchedule[2] = config_.TCS_SL_NLPerc_startBound_Rear; + in.TCS_SL_Targets_NLSchedule[3] = config_.TCS_SL_NLPerc_endBound_Rear; + + in.useNL_TCS_SlipSchedule = config_.useNL_TCS_SlipSchedule; + if ((vn_active_start_time_ == 0) && (vn_status >= 2)) { vn_active_start_time_ = tick.millis; @@ -182,6 +194,7 @@ DrivetrainCommand_s CASESystem::evaluate( enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl); last_controller_pt1_send_time_ = tick.millis; } @@ -237,6 +250,7 @@ DrivetrainCommand_s CASESystem::evaluate( enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp); enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl); + enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tc_pna); last_lowest_priority_controller_send_time_ = tick.millis; } diff --git a/platformio.ini b/platformio.ini index 976d82316..c2c9edab3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -56,8 +56,8 @@ lib_deps = https://github.com/RCMast3r/spi_libs https://github.com/tonton81/FlexCAN_T4 https://github.com/RCMast3r/hytech_can#testing_new_inv_ids - https://github.com/hytech-racing/HT_CAN/releases/download/96/can_lib.tar.gz - git+ssh://git@github.com/hytech-racing/CASE_lib.git#v48 + https://github.com/hytech-racing/HT_CAN/releases/download/97/can_lib.tar.gz + git+ssh://git@github.com/hytech-racing/CASE_lib.git#v49 [env:test_can_on_teensy] lib_ignore = diff --git a/src/main.cpp b/src/main.cpp index 0e71e20df..e50e4d274 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -158,7 +158,7 @@ CASEConfiguration case_config = { .yaw_pid_p = 1.5, .yaw_pid_i = 0.25, .yaw_pid_d = 0.0, - .tcs_pid_p_lowerBound_front = 35.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error + .tcs_pid_p_lowerBound_front = 55.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error .tcs_pid_p_upperBound_front = 45.0, .tcs_pid_p_lowerBound_rear = 32.0, .tcs_pid_p_upperBound_rear = 45.0, @@ -173,8 +173,6 @@ CASEConfiguration case_config = { .usePIDPowerLimit = false, .useDecoupledYawBrakes = true, .useDiscontinuousYawPIDBrakes = true, - .tcsSLThreshold = 0.3, - .launchSL = 0.3, .launchDeadZone = 20.0, // N-m .launchVelThreshold = 0.15, // m/s .tcsVelThreshold = 1.5, // m/s @@ -212,6 +210,19 @@ CASEConfiguration case_config = { .TCS_NL_endBoundPerc_FrontAxle = 0.4, .TCS_NL_startBoundPerc_RearAxle = 0.5, .TCS_NL_endBoundPerc_RearAxle = 0.6, + .useNL_TCS_SlipSchedule = true, + .launchSL_startBound_Front = 0.25, + .launchSL_endBound_Front = 0.15, + .launchSL_startBound_Rear = 0.3, + .launchSL_endBound_Rear = 0.4, + .TCS_SL_startBound_Front = 0.25, + .TCS_SL_endBound_Front = 0.15, + .TCS_SL_startBound_Rear = 0.3, + .TCS_SL_endBound_Rear = 0.4, + .TCS_SL_NLPerc_startBound_Front = 0.5, + .TCS_SL_NLPerc_endBound_Front = 0.4, + .TCS_SL_NLPerc_startBound_Rear = 0.5, + .TCS_SL_NLPerc_endBound_Rear = 0.6, // Following used for calculate_torque_request in CASESystem.tpp .max_rpm = 20000,