started work on cleaning up pedals system and put the torque control …

…mux in the state machine
hytech-racing · Feb 7, 2024 · 71ededb · 71ededb
1 parent 5b51261
commit 71ededb
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 15 deletions.
diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp
@@ -25,7 +25,6 @@ void InverterInterface<message_queue>::request_enable_hv()
 template <typename message_queue>
 void InverterInterface<message_queue>::request_enable_inverter()
 {
-
     MC_setpoints_command mc_setpoints_command{};
     mc_setpoints_command.set_speed_setpoint(0);
     mc_setpoints_command.set_pos_torque_limit(0);

diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h
@@ -27,13 +27,18 @@ template <typename DrivetrainSysType>
 class MCUStateMachine
 {
 public:
-    MCUStateMachine(BuzzerController *buzzer, DrivetrainSysType *drivetrain, DashboardInterface *dashboard, PedalsSystem *pedals)
+    MCUStateMachine(BuzzerController *buzzer,
+                    DrivetrainSysType *drivetrain,
+                    DashboardInterface *dashboard,
+                    PedalsSystem *pedals,
+                    TorqueControllerMux *mux)
     {
         current_state_ = CAR_STATE::STARTUP;
         buzzer_ = buzzer;
         drivetrain_ = drivetrain;
         dashboard_ = dashboard;
         pedals_ = pedals;
+        controller_mux_ = mux;
     }
 
     /// @brief our components can use this time to tell when to do things. We can set this ourselves for testing purposes instead of using metro timers
@@ -43,7 +48,6 @@ class MCUStateMachine
     CAR_STATE get_state() { return current_state_; }
 
 private:
-
     void set_state_(CAR_STATE new_state, unsigned long curr_time);
 
     /// @brief the function run upon the entry of the car into a new state
@@ -65,8 +69,7 @@ class MCUStateMachine
     AMSInterface *bms_;
     // IMDInterface *imd_;
 
-    TorqueControllerMux* controller_mux_;
-
+    TorqueControllerMux *controller_mux_;
 };
 #include "MCUStateMachine.tpp"
 #endif /* MCUSTATEMACHINE */
diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp
@@ -74,7 +74,6 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
     {
         auto data = pedals_->getPedalsSystemData();
 
-        hal_println("RTD");
         if (!drivetrain_->hv_over_threshold_on_drivetrain())
         {
             hal_println("drivetrain not over thresh?");
@@ -87,18 +86,14 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
             set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis);
             break;
         }
-        // TODO migrate the handling of the pedals / move to the new pedals system
-        // PedalsDriverInterface data;
-        // auto pedals_data = pedals_->evaluate_pedals(data, current_millis);
-        // auto dashboard_data = dashboard_->evaluate_dashboard(dash_data);
 
-        // TODO: below in the scope of this function
+        // TODO:integrate the ams and the IMD
         if (
             // bms_->ok_high() &&
             // imd_->ok_high() &&
             !data.implausibilityExceededMaxDuration)
         {
-            // drivetrain_->command_drivetrain(controller_mux_->get_drivetrain_input(pedals_data, dashboard_data));
+            drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand());
         }
         else
         {

diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h
@@ -61,12 +61,17 @@ class PedalsSystem
     bool evaluate_brake_and_accel_pressed_();
 
     bool pedal_is_active_(int sense_1, int sense_2, const PedalsSystemParameters_s &pedalParams, float percent_threshold);
-
+    
 public:
 // Constructors
-    PedalsSystem(PedalsSystemParameters_s* parametersExt)
+    PedalsSystem(PedalsSystemParameters_s parametersExt)
     {
-        parameters_ = *parametersExt;
+        parameters_ = parametersExt;
+    }
+
+    void update_parameters(const PedalsSystemParameters_s &new_params)
+    {
+        parameters_ = new_params;
     }
     PedalsSystem()
     {
@@ -89,6 +94,8 @@ class PedalsSystem
         const AnalogConversion_s &brake1, 
         const AnalogConversion_s &brake2
     );
+
+
     const PedalsSystemData_s& getPedalsSystemData()
     {
         return data_;