addressing comments

hytech-racing · Sep 21, 2024 · d857f81 · d857f81
1 parent 53d653a
commit d857f81
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 106 deletions.
diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h
@@ -73,7 +73,6 @@ class MCUStateMachine
     // IMDInterface *imd_;
     SafetySystem *safety_system_;
     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
@@ -13,41 +13,6 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
 
     case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE:
     {
-        // Serial.println("eror in ts not active:");
-        // Serial.println(safety_system_->get_software_is_ok());
-        // Serial.println();
-        // hal_println("tractive system not active state");
-
-        // auto data = pedals_->getPedalsSystemData();
-        // auto mux_test = controller_mux_->getDrivetrainCommand();
-
-        // hal_println("speeds 1 through 4");
-        // Serial.println(mux_test.speeds_rpm[0]);
-        // Serial.println(mux_test.speeds_rpm[1]);
-        // Serial.println(mux_test.speeds_rpm[2]);
-        // Serial.println(mux_test.speeds_rpm[3]);
-
-        // hal_println("torqeus");
-        // Serial.println(mux_test.torqueSetpoints[0]);
-        // Serial.println(mux_test.torqueSetpoints[1]);
-        // Serial.println(mux_test.torqueSetpoints[2]);
-        // Serial.println(mux_test.torqueSetpoints[3]);
-        // Serial.println();
-        // Serial.print(data.brakeImplausible);
-        // Serial.print(" ");
-        // Serial.print(data.accelImplausible);
-        // Serial.print(" ");
-        // Serial.print(data.brakeAndAccelPressedImplausibility);
-        // Serial.print(" ");
-
-        // Serial.println("accel, brake:");
-        // Serial.print(data.accelPercent);
-        // Serial.print(" ");
-        // Serial.print(data.brakePercent);
-        // Serial.print(" \n");
-        // 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())
@@ -63,7 +28,6 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
         {
             buzzer_->deactivate();
         }
-        // hal_println("in tractive system active state");
         // TODO migrate to new pedals system
         auto data = pedals_->getPedalsSystemData();
         drivetrain_->disable_no_pins();
@@ -83,7 +47,6 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
     case CAR_STATE::ENABLING_INVERTERS:
     {
 
-        // hal_println("in enabling inverters state");
         if (!drivetrain_->hv_over_threshold_on_drivetrain())
         {
             set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis);
@@ -144,8 +107,6 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
 
         if (safety_system_->get_software_is_ok() && !data.implausibilityExceededMaxDuration)
         {
-            // logger_.log_out("torque mode:", current_millis, 100);
-            // logger_.log_out(static_cast<int>(dashboard_->getTorqueLimitMode()), current_millis, 100);
             drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand(dashboard_->getDialMode(), dashboard_->getTorqueLimitMode(), current_car_state));
         }
         else
@@ -161,14 +122,11 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
 template <typename DrivetrainSysType>
 void MCUStateMachine<DrivetrainSysType>::set_state_(CAR_STATE new_state, unsigned long curr_time)
 {
-    // hal_println("running exit logic");
 
-    // hal_printf("\n to state: %i\n", new_state);
     handle_exit_logic_(current_state_, curr_time);
 
     current_state_ = new_state;
 
-    // hal_println("running entry logic");
     handle_entry_logic_(new_state, curr_time);
 }
 
@@ -215,13 +173,10 @@ void MCUStateMachine<DrivetrainSysType>::handle_entry_logic_(CAR_STATE new_state
     {
         // make dashboard sound buzzer
         buzzer_->activate_buzzer(curr_time);
-        // hal_println("RTDS enabled");
         break;
     }
     case CAR_STATE::READY_TO_DRIVE:
     {
-        // hal_printf("%i\n", curr_time);
-        // hal_println("Ready to drive");
         break;
     }
     }

diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp
@@ -200,71 +200,71 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
 
     // send these out at the send period
 
-    // if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_))
-    // {
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p);
-
-    //     last_controller_pt1_send_time_ = tick.millis;
-    // }
+    if ((tick.millis - last_controller_pt1_send_time_) >= (controller_send_period_ms_))
+    {
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_initia);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_norm_p);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pow_pn);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_to);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_regen_);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p);
+
+        last_controller_pt1_send_time_ = tick.millis;
+    }
 
-    // if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
-    //     ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_))
-    // {
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn);
-
-    //     last_controller_pt2_send_time_ = tick.millis;
-    // }
+    if (((tick.millis - last_controller_pt1_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
+        ((tick.millis - last_controller_pt2_send_time_) > controller_send_period_ms_))
+    {
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_normal);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pi);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs__p);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_st);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_pn);
+
+        last_controller_pt2_send_time_ = tick.millis;
+    }
 
-    // if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
-    //     ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_))
-    // {
+    if (((tick.millis - last_controller_pt2_send_time_) >= (vehicle_math_offset_ms_ / 3)) &&
+        ((tick.millis - last_controller_pt3_send_time_) > controller_send_period_ms_))
+    {
 
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sl);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_rege_p);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_torque);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_power_);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_powe_p);
 
-    //     last_controller_pt3_send_time_ = tick.millis;
-    // }
+        last_controller_pt3_send_time_ = tick.millis;
+    }
 
-    // if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) &&
-    //     ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_))
-    // {
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve);
-
-    //     last_vehm_send_time_ = tick.millis;
-    // }
+    if (((tick.millis - last_controller_pt1_send_time_) >= vehicle_math_offset_ms_) &&
+        ((tick.millis - last_vehm_send_time_) > controller_send_period_ms_))
+    {
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_alpha_deg);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_sl);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_long_corner_);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_steer_);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_kin_desired_);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_beta_deg);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_vehm_wheel_lin_ve);
+
+        last_vehm_send_time_ = tick.millis;
+    }
 
-    // if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_))
-    // {
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi);
-    //     enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa);
-    //     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;
-    // }
+    if ((tick.millis - last_lowest_priority_controller_send_time_) >= (lowest_priority_controller_send_period_ms_))
+    {
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_boolea);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_co);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa);
+        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;
+    }
 
     DrivetrainCommand_s command;
 

diff --git a/lib/systems/src/DrivebrainController.cpp b/lib/systems/src/DrivebrainController.cpp
@@ -1,5 +1,4 @@
 #include "DrivebrainController.h"
-#include <algorithm> // for std::copy
 
 
 TorqueControllerOutput_s DrivebrainController::evaluate(const SharedCarState_s &state)

diff --git a/src/main.cpp b/src/main.cpp
@@ -568,6 +568,7 @@ void tick_all_interfaces(const SysTick_s &current_system_tick)
 
         PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy();
         front_thermistors_interface.tick(mcu_adc.get().conversions[MCU15_THERM_FL_CHANNEL], mcu_adc.get().conversions[MCU15_THERM_FR_CHANNEL]);
+        // TODO pass in the shared state instead
         telem_interface.tick(
             a1.get(),
             a2.get(),
@@ -587,6 +588,7 @@ void tick_all_interfaces(const SysTick_s &current_system_tick)
             a1.get().conversions[MCU15_BRAKE2_CHANNEL],
             pedals_system.getMechBrakeActiveThreshold(),
             torque_controller_mux.get_tc_mux_status().current_error);
+
         ams_interface.tick(current_system_tick);
     }