Refactor PID class and PID node

StrayedCats · Feb 23, 2024 · 779f97a · 779f97a
1 parent c08c5dd
commit 779f97a
Show file tree

Hide file tree

Showing 2 changed files with 72 additions and 75 deletions.
diff --git a/auto_driver_interface/src/pid.hpp b/auto_driver_interface/src/pid.hpp
@@ -4,62 +4,59 @@ class PID
 {
 
 public:
-    PID(float p, float i, float d, float max, float min, float dt)
-    {
-        this->p = p;
-        this->i = i;
-        this->d = d;
-        this->max = max;
-        this->min = min;
-        this->integral = 0;
-        this->previous_error = 0;
-        this->dt = dt;
-    }
+  PID(float p, float i, float d, float max, float min, float dt)
+  {
+    this->p = p;
+    this->i = i;
+    this->d = d;
+    this->max = max;
+    this->min = min;
+    this->integral = 0;
+    this->previous_error = 0;
+    this->dt = dt;
+  }
 
-    float calculate(float setpoint, float pv)
-    {
-        float error = setpoint - pv;
-        float p_term = p * error;
-        integral += error * dt;
-        float i_term = i * integral;
-        float d_term = d * ((error - previous_error) / dt);
-        previous_error = error;
-        float output = p_term + i_term + d_term;
-        if (output > max)
-        {
-            output = max;
-        }
-        else if (output < min)
-        {
-            output = min;
-        }
-        return output;
+  float calculate(float setpoint, float pv)
+  {
+    float error = setpoint - pv;
+    float p_term = p * error;
+    integral += error * dt;
+    float i_term = i * integral;
+    float d_term = d * ((error - previous_error) / dt);
+    previous_error = error;
+    float output = p_term + i_term + d_term;
+    if (output > max) {
+      output = max;
+    } else if (output < min) {
+      output = min;
     }
+    return output;
+  }
 
-    void reconfigure(float p, float i, float d, float max, float min)
-    {
-        this->p = p;
-        this->i = i;
-        this->d = d;
-        this->max = max;
-        this->min = min;
+  void reconfigure(float p, float i, float d, float max, float min)
+  {
+    this->p = p;
+    this->i = i;
+    this->d = d;
+    this->max = max;
+    this->min = min;
 
-        reset();
-    }
+    reset();
+  }
 
-    void reset()
-    {
-        integral = 0;
-        previous_error = 0;
-    }
+  void reset()
+  {
+    integral = 0;
+    previous_error = 0;
+  }
 
 private:
-    float p;
-    float i;
-    float d;
-    float max;
-    float min;
-    float integral;
-    float previous_error;
-    float dt;
+  float p;
+  float i;
+  float d;
+  float max;
+  float min;
+  float integral;
+  float previous_error;
+  float dt;
 };
diff --git a/auto_driver_interface/src/pid_node.cpp b/auto_driver_interface/src/pid_node.cpp
@@ -48,37 +48,37 @@ class PidNode : public rclcpp::Node
 
     publisher_ = this->create_publisher<std_msgs::msg::Int64>("target_volt", 1);
 
-    current_deg_sub = this->create_subscription<std_msgs::msg::Float64>("degree", 1, [&](const std_msgs::msg::Float64::SharedPtr msg) {
+    current_deg_sub = this->create_subscription<std_msgs::msg::Float64>(
+      "degree", 1, [&](const std_msgs::msg::Float64::SharedPtr msg) {
         current_deg = msg->data;
-    });
-    target_deg_sub = this->create_subscription<std_msgs::msg::Int64>("target_deg", 1, [&](const std_msgs::msg::Int64::SharedPtr msg) {
-      if (msg->data > max_limit)
-      {
-        RCLCPP_WARN(this->get_logger(), "Target degree is over max limit");
-        target_deg = max_limit;
-      }
-      else if (msg->data < min_limit)
-      {
-        RCLCPP_WARN(this->get_logger(), "Target degree is under min limit");
-        target_deg = min_limit;
-      }
-      else
-      {
-        target_deg = msg->data;
-      }
-    });
-
-    timer_ = this->create_wall_timer(std::chrono::milliseconds(5), std::bind(&PidNode::timer_callback, this));
+      });
+    target_deg_sub = this->create_subscription<std_msgs::msg::Int64>(
+      "target_deg", 1, [&](const std_msgs::msg::Int64::SharedPtr msg) {
+        if (msg->data > max_limit) {
+          RCLCPP_WARN(this->get_logger(), "Target degree is over max limit");
+          target_deg = max_limit;
+        } else if (msg->data < min_limit) {
+          RCLCPP_WARN(this->get_logger(), "Target degree is under min limit");
+          target_deg = min_limit;
+        } else {
+          target_deg = msg->data;
+        }
+      });
+
+    timer_ =
+      this->create_wall_timer(
+      std::chrono::milliseconds(5),
+      std::bind(&PidNode::timer_callback, this));
     pid = std::make_shared<PID>(p, i, d, max_spd, -max_spd, 0.005);
-    
+
   }
 
 private:
   void timer_callback()
   {
-      auto message = std_msgs::msg::Int64();
-      message.data = pid->calculate(target_deg, current_deg);
-      publisher_->publish(message);
+    auto message = std_msgs::msg::Int64();
+    message.data = pid->calculate(target_deg, current_deg);
+    publisher_->publish(message);
   }
 
   rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr publisher_;