FBLin PID finally working again. Cleanup still required

vanttec · Jan 29, 2025 · 8db9ac5 · 8db9ac5
1 parent 30f45e1
commit 8db9ac5
Show file tree

Hide file tree

Showing 18 changed files with 10 additions and 201 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -30,6 +30,7 @@ target_sources(vanttec_controllers
   src/controllers/control_laws/PID/first_order/pid.cpp
   src/controllers/feedback_linearization/base/fb_lin_control.cpp
   src/controllers/feedback_linearization/PID/first_order/fblin_pid.cpp
+  src/controllers/feedback_linearization/model_based/SDCs/regulators/vtec_sdc1_pid.cpp
 
   src/dynamic_models/ground_vehicles/car_like/base/car_3dof_dynamic_model.cpp
   src/dynamic_models/ground_vehicles/car_like/vehicles/vtec_sdc1.cpp

diff --git a/src/controllers/control_laws/PID/first_order/pid.cpp b/src/controllers/control_laws/PID/first_order/pid.cpp
@@ -11,7 +11,6 @@
 #include "controllers/control_laws/PID/first_order/pid.hpp"
 #include <algorithm>
 #include <cmath>
-#include <iostream>
 
 PID::PID(const PIDParameters &params)
 { 

diff --git a/src/controllers/feedback_linearization/PID/first_order/fblin_pid.cpp b/src/controllers/feedback_linearization/PID/first_order/fblin_pid.cpp
@@ -9,7 +9,6 @@
  * */
 
 #include "controllers/feedback_linearization/PID/first_order/fblin_pid.hpp"
-#include <iostream>
 
 PIDLin::PIDLin(double FB_LIN_UMAX, double FB_LIN_UMIN, const PIDParameters &params) : 
                FBLin (FB_LIN_UMAX, FB_LIN_UMIN),

diff --git a/...ed_controllers/SDCs/path_following/DELETE → ...on/model_based/SDCs/path_following/DELETE b/...ed_controllers/SDCs/path_following/DELETE → ...on/model_based/SDCs/path_following/DELETE
diff --git a/...lers/SDCs/regulators/vtec_sdc1_aitsmc.cpp → ...ased/SDCs/regulators/vtec_sdc1_aitsmc.cpp b/...lers/SDCs/regulators/vtec_sdc1_aitsmc.cpp → ...ased/SDCs/regulators/vtec_sdc1_aitsmc.cpp
diff --git a/...lers/SDCs/regulators/vtec_sdc1_aitsmc.hpp → ...ased/SDCs/regulators/vtec_sdc1_aitsmc.hpp b/...lers/SDCs/regulators/vtec_sdc1_aitsmc.hpp → ...ased/SDCs/regulators/vtec_sdc1_aitsmc.hpp
diff --git a/...ollers/SDCs/regulators/vtec_sdc1_asmc.cpp → ..._based/SDCs/regulators/vtec_sdc1_asmc.cpp b/...ollers/SDCs/regulators/vtec_sdc1_asmc.cpp → ..._based/SDCs/regulators/vtec_sdc1_asmc.cpp
diff --git a/...ollers/SDCs/regulators/vtec_sdc1_asmc.hpp → ..._based/SDCs/regulators/vtec_sdc1_asmc.hpp b/...ollers/SDCs/regulators/vtec_sdc1_asmc.hpp → ..._based/SDCs/regulators/vtec_sdc1_asmc.hpp
diff --git a/...rollers/SDCs/regulators/vtec_sdc1_pid.cpp → ...l_based/SDCs/regulators/vtec_sdc1_pid.cpp b/...rollers/SDCs/regulators/vtec_sdc1_pid.cpp → ...l_based/SDCs/regulators/vtec_sdc1_pid.cpp
@@ -11,14 +11,14 @@
 
 #include "vtec_sdc1_pid.hpp"
 
-VTEC_SDC1_1DOF_PID::VTEC_SDC1_1DOF_PID(const PIDParameters &params, VTecSDC1DynamicModel* model)
-    : PIDLin(params.kUMax, params.kUMin, params) {}
+VTEC_SDC1_1DOF_PID::VTEC_SDC1_1DOF_PID(const PIDParameters &params, std::shared_ptr<VTecSDC1DynamicModel> model)
+    : PIDLin(params.kUMax, params.kUMin, params), sdc1_model_(model) {}
 
 VTEC_SDC1_1DOF_PID::~VTEC_SDC1_1DOF_PID() {}
 
 void VTEC_SDC1_1DOF_PID::updateNonLinearFunctions() {
-  PIDLin::f_x_ = model->f_(0);
-  PIDLin::g_x_ = model->g_(0);
+  PIDLin::f_x_ = sdc1_model_->f_(0);
+  PIDLin::g_x_ = sdc1_model_->g_(0);
 }
 
 double VTEC_SDC1_1DOF_PID::calculateControlSignals(double chi1, double chi1_d, double chi1_dot_d) {
@@ -30,5 +30,5 @@ double VTEC_SDC1_1DOF_PID::calculateControlSignals(double chi1, double chi1_d, d
 }
 
 void VTEC_SDC1_1DOF_PID::updateControlSignals() {
-  model->u_(0) = PIDLin::u_;
+  sdc1_model_->u_(0) = PIDLin::u_;
 }
diff --git a/...rollers/SDCs/regulators/vtec_sdc1_pid.hpp → ...l_based/SDCs/regulators/vtec_sdc1_pid.hpp b/...rollers/SDCs/regulators/vtec_sdc1_pid.hpp → ...l_based/SDCs/regulators/vtec_sdc1_pid.hpp
@@ -12,10 +12,11 @@
 #include "controllers/feedback_linearization/PID/first_order/fblin_pid.hpp"
 #include "dynamic_models/ground_vehicles/car_like/vehicles/vtec_sdc1.hpp"
 #include "utils/utils.hpp"
+#include <memory>
 
 class VTEC_SDC1_1DOF_PID : public PIDLin {
   public:
-    VTEC_SDC1_1DOF_PID(const PIDParameters &params, VTecSDC1DynamicModel* model);
+    VTEC_SDC1_1DOF_PID(const PIDParameters &params, std::shared_ptr<VTecSDC1DynamicModel> model);
     ~VTEC_SDC1_1DOF_PID();
 
     void updateNonLinearFunctions();
@@ -24,5 +25,5 @@ class VTEC_SDC1_1DOF_PID : public PIDLin {
 
     void updateControlSignals();
 
-    VTecSDC1DynamicModel* model;
+    std::shared_ptr<VTecSDC1DynamicModel> sdc1_model_; 
 };
diff --git a/...ntrollers/SDCs/trajectory_trackers/DELETE → ...del_based/SDCs/trajectory_trackers/DELETE b/...ntrollers/SDCs/trajectory_trackers/DELETE → ...del_based/SDCs/trajectory_trackers/DELETE
diff --git a/..._trackers/vtec_u4_6dof_in_pid_tracker.cpp → ..._trackers/vtec_u4_6dof_in_pid_tracker.cpp b/..._trackers/vtec_u4_6dof_in_pid_tracker.cpp → ..._trackers/vtec_u4_6dof_in_pid_tracker.cpp
@@ -8,7 +8,7 @@
  * -----------------------------------------------------------------------------
  **/
 
-#include "controllers/feedback_linearization/model_based_controllers/UUVs/vtec_u4_6dof_in_pid.hpp"
+#include "controllers/feedback_linearization/model_based/UUVs/vtec_u4_6dof_in_pid.hpp"
 
 VTEC_U4_6DOF_PID_TRACKER::VTEC_U4_6DOF_PID_TRACKER(float sample_time, const std::vector<float>& k_p, const std::vector<float>& k_i, 
         const std::vector<float>& k_d, const std::array<float,6>& u_max,

diff --git a/..._trackers/vtec_u4_6dof_in_pid_tracker.hpp → ..._trackers/vtec_u4_6dof_in_pid_tracker.hpp b/..._trackers/vtec_u4_6dof_in_pid_tracker.hpp → ..._trackers/vtec_u4_6dof_in_pid_tracker.hpp
diff --git a/src/dynamic_models/ground_vehicles/car_like/base/car_3dof_dynamic_model.cpp b/src/dynamic_models/ground_vehicles/car_like/base/car_3dof_dynamic_model.cpp
@@ -34,7 +34,6 @@
 
 #include "car_3dof_dynamic_model.hpp"
 
-#include <iostream>
 #include "utils/utils.hpp"
 #include "utils/utils.cpp"
 

diff --git a/src/simulation/base/tf2_6dof_broadcaster_ros2.cpp b/src/simulation/base/tf2_6dof_broadcaster_ros2.cpp
diff --git a/src/simulation/base/tf2_6dof_broadcaster_ros2.hpp b/src/simulation/base/tf2_6dof_broadcaster_ros2.hpp
diff --git a/src/simulation/car_tfs/sdc1_broadcaster.cpp b/src/simulation/car_tfs/sdc1_broadcaster.cpp
diff --git a/src/simulation/car_tfs/sdc1_broadcaster.hpp b/src/simulation/car_tfs/sdc1_broadcaster.hpp