Merge pull request #75 from hytech-racing/update_slip_launch_logic

Update slip launch to remove delay. Merging because this is simple change that only pertains to one controller. Trying to clean up all of these random PRs now that we have time to test.
hytech-racing · Apr 16, 2024 · 047a46e · 047a46e
2 parents 9899dee + 373be51
commit 047a46e
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 17 deletions.
diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h
@@ -33,7 +33,7 @@ const float DEFAULT_LAUNCH_RATE = 11.76;
 const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500;
 
 const float DEFAULT_SLIP_RATIO = 0.2f;
-const float const_accel_time = 1500; // time to use launch speed target in ms
+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;

diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp
@@ -283,23 +283,22 @@ void TorqueControllerSlipLaunch::calc_launch_algo(const vector_nav* vn_data) {
             uint32_t ms_since_launch = (current_millis - time_of_launch);
             // accelerate at constant speed for a period of time to get body velocity up
             // may want to make this the ht07 launch algo
-            if(ms_since_launch < const_accel_time){
-                launch_speed_target = DEFAULT_LAUNCH_SPEED_TARGET;
-
-            } else {
-
-                /*
-                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;
-                launch_speed_target = std::max(launch_speed_target, new_speed_target);
+
+            // 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);
 
 }