Merge branch 'CASE_testbranch' into feature/filter-steering-reading

hytech-racing · May 21, 2024 · b528dfc · b528dfc
2 parents f4e69ef + 207b299
commit b528dfc
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 13 deletions.
diff --git 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
@@ -21,7 +21,9 @@ DrivetrainCommand_s CASESystem<message_queue>::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<message_queue>::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<message_queue>::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<message_queue>::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<message_queue>::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
@@ -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
@@ -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,