feat(controller): motor controller working, but not fine tuned

Author: kpbaks

arc_base/src/low_level_motor_controller.cpp

@@ -65,6 +65,21 @@ class MotorController : public ros2::Node {
     Publisher<Float64Msg>::SharedPtr motor_pub_;
     Publisher<MotorControllerStatusMsg>::SharedPtr status_pub_;
 
+    void pub_status_() {
+        // update message
+
+        msg_status_.control.target = target_;
+        msg_status_.control.actual = actual_;
+        msg_status_.control.error = error_;
+        msg_status_.pid.p = control_pid_.p;
+        msg_status_.pid.i = control_pid_.i;
+        msg_status_.pid.d = control_pid_.d;
+        msg_status_.output = output_;
+
+        status_pub_->publish(msg_status_);
+        // RCLCPP_DEBUG(this->get_logger(), "published status");
+    }
+
   public:
     MotorController()
         : Node("motor_controller"), control_pid_{0.0, 0.0, 0.0},
@@ -186,7 +201,6 @@ class MotorController : public ros2::Node {
         diff_drive_sub_ = this->create_subscription<Float64Msg>(
             diff_drive_topic.c_str(), 10, [this](Float64Msg::UniquePtr msg) {
                 this->target_ = this->get_target_value_(*msg);
-                this->actual_ = this->get_sensor_value_(vesc_state_);
             });
 
         const String state_topic =
@@ -197,6 +211,7 @@ class MotorController : public ros2::Node {
             state_topic.c_str(), 10,
             [this](VescStateStampedMsg::UniquePtr msg) {
                 this->vesc_state_ = *msg;
+                this->actual_ = this->get_sensor_value_(*msg);
             });
 
         // publishers
@@ -228,20 +243,8 @@ class MotorController : public ros2::Node {
             motor_pub_->publish(msg_control_);
         });
 
-        timer_status_ = this->create_wall_timer(cooldown, [this]() {
-            // update message
-
-            msg_status_.control.target = target_;
-            msg_status_.control.actual = actual_;
-            msg_status_.control.error = error_;
-            msg_status_.pid.p = control_pid_.p;
-            msg_status_.pid.i = control_pid_.i;
-            msg_status_.pid.d = control_pid_.d;
-            msg_status_.output = output_;
-
-            status_pub_->publish(msg_status_);
-            // RCLCPP_DEBUG(this->get_logger(), "published status");
-        });
+        timer_status_ =
+            this->create_wall_timer(cooldown, [this]() { pub_status_(); });
     }
 
     void step() {
@@ -277,14 +280,16 @@ class MotorController : public ros2::Node {
 
         // In (0, 450) we want to clamp to 900
         // In [450, 900) we want to clamp to 0
-        if (std::abs(output_) < 450) {
-            output_ =
-                output_ == 0 ? 0 : (output_ > 0 ? speed_min_ : -speed_min_);
-        } else if (std::abs(output_) < 900) {
+        if (std::abs(output_) < 450) { // In (0, 450)
+            output_ = std::abs(output_) < std::numeric_limits<double>::epsilon()
+                          ? 0
+                          : std::copysign(speed_min_, output_);
+        } else if (std::abs(output_) < 900) { // In [450, 900)
             output_ = 0;
             integral_ = 0;
             error_ = 0;
-            // std::this_thread::sleep_for(500ms);
+            // actual_ = 0;
+            pub_status_();
             // ros2::sleep_for(500ms);
         }
 

arc_base/src/twist_to_diff_drive_node.cpp

@@ -1,8 +1,8 @@
+#include "arc_base/utils.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float64.hpp"
-#include "arc_base/utils.hpp"
 #include <cmath>
 #include <cstdio>
 #include <iostream>
@@ -73,12 +73,14 @@ class DifferentialDrive {
 
 class TwistToDiffDriveNode : public rclcpp::Node {
   public:
-    TwistToDiffDriveNode(double rate)
-        : Node("twist_to_diff_drive_node"), rate_{rate},
+    TwistToDiffDriveNode()
+        : Node("twist_to_diff_drive_node"),
           diff_drive_model_(0.224, 0.15, GEARING, 1) {
         // Create a DifferentialDrive object with a wheel base of 224mm and a
         // wheel radius of 150mm
 
+        this->declare_parameter("controller_rate"); // Hz
+
         // twist command subscriber
         twist_sub_ = this->create_subscription<TwistStampedMsg>(
             "cmd_vel", 10,
@@ -92,26 +94,32 @@ class TwistToDiffDriveNode : public rclcpp::Node {
     }
 
     void spin() {
+        double controller_rate =
+            this->get_parameter("controller_rate").as_double();
+        std::fprintf(stderr, "%s: %.5f\n", magenta("controller_rate").c_str(),
+                     controller_rate);
+        ros2::Rate rate(controller_rate);
+
         while (rclcpp::ok()) {
             // publish the left and right motor velocities
             if (publish_motor_speed(MotorSide::LEFT, left_motor_speed_)) {
                 // RCLCPP_INFO(this->get_logger(), "Published left motor speed:
                 // %0.5f", left_motor_speed_);
             }
             rclcpp::spin_some(this->get_node_base_interface());
-            rate_.sleep();
+            rate.sleep();
 
             if (publish_motor_speed(MotorSide::RIGHT, right_motor_speed_)) {
                 // RCLCPP_INFO(this->get_logger(), "Published right motor speed:
                 // %0.5f", right_motor_speed_);
             }
             rclcpp::spin_some(this->get_node_base_interface());
-            rate_.sleep();
+            rate.sleep();
         }
     }
 
   private:
-    rclcpp::Rate rate_;
+    // rclcpp::Rate rate_;
     Subscriber<TwistStampedMsg>::SharedPtr twist_sub_;
     Publisher<Float64Msg>::SharedPtr motor_speed_left_pub_;
     Publisher<Float64Msg>::SharedPtr motor_speed_right_pub_;
@@ -185,7 +193,7 @@ int main(int argc, char **argv) {
 
     // rclcpp::executors::MultiThreadedExecutor executor;
 
-    auto node = std::make_shared<arc::TwistToDiffDriveNode>(10);
+    auto node = std::make_shared<arc::TwistToDiffDriveNode>();
     node->spin();
     // executor.add_node(node);
 

arc_bringup/config/params.yaml

@@ -6,7 +6,7 @@ low_level_motor_controller_left:
     current_max: 2.0
     speed_min: 900.0
     speed_max: 23000.0
-    acc_max: 0.1
+    acc_max: 5000.0
     kp_speed: 1.0 # 0.004
     ki_speed: 0.0 # 0.004
     kd_speed: 0.0 # 0.0001
@@ -22,10 +22,14 @@ low_level_motor_controller_right:
     current_max: 2.0
     speed_min: 900.0
     speed_max: 23000.0
-    acc_max: 0.1
+    acc_max: 5000.0
     kp_speed: 1.0 # 0.004
     ki_speed: 0.0 # 0.004
     kd_speed: 0.0 # 0.0001
     kp_current: 0.004
     ki_current: 0.004
     kd_current: 0.0001
+
+twist_to_diff_drive:
+  ros__parameters:
+    controller_rate: 1000.0

arc_bringup/launch/test-setup.launch.py

@@ -94,19 +94,20 @@ def generate_launch_description():
         )
     )
 
-    # twist to diff drive
-    twist_to_diff_drive = Node(
-        package='arc_base',
-        executable='twist_to_diff_drive_node',
-        name='twist_to_diff_drive'
-    )
-
     config = os.path.join(
         get_package_share_directory('arc_bringup'),
         'config',
         'params.yaml'
     )
 
+    # twist to diff drive
+    twist_to_diff_drive = Node(
+        package='arc_base',
+        executable='twist_to_diff_drive_node',
+        name='twist_to_diff_drive',
+        parameters=[config]
+    )
+
     print(green("config: " + config))
 
     # low level motor controller for left motor