“~”

catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp

@@ -30,7 +30,7 @@ public:
 };
 
 
-PIDController pid(2, 0, 5);
+PIDController pid(3, 0, 5);
 PIDController pid1(40, 0, 0);
 PIDController pid_v(1.5, 0, 0);
 

catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp

@@ -3,8 +3,6 @@
 #include <cmath>
 #include "asclinic_pkg/LeftRightInt32.h"
 #include "asclinic_pkg/LeftRightFloat32.h"
-#include "asclinic_pkg/position.h"
-#include "asclinic_pkg/vw.h"
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
 #include <nav_msgs/Odometry.h>
@@ -25,7 +23,6 @@ constexpr float WHEEL_RADIUS_RIGHT = 0.072; // Right wheel radius in meters
 constexpr float HALF_WHEEL_BASE = 0.125;    // Half of the wheelbase in meters
 constexpr float k_l = 0.00023;  // Uncertainty in the left wheel
 constexpr float k_r = 0.0001;  // Uncertainty in the right wheel
-int authority =0;
 int left_p = 1, right_p = 1;
 
 using Eigen::MatrixXd;
@@ -49,7 +46,20 @@ float normalize_angle(float angle) {
         return 2 * M_PI - value;
     }
 }
-
+void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
+    if(msg.left<0){
+        left_p = -1;
+    }
+    else{
+        left_p = 1;
+    }
+    if(msg.right<0){
+        right_p = -1;
+    }
+    else{
+        right_p = 1;
+    }
+}
 
 void answer(const asclinic_pkg::LeftRightInt32& msg) {
     // Update times for elapsed time calculation
@@ -60,8 +70,8 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
     ROS_INFO_STREAM("the seperation in second is "<<time_float);
 
     // Calculate angular displacements for left and right wheels
-    float deltaThetaL = 0.000404 * msg.left / WHEEL_RADIUS_LEFT; // Calculate rotation angle for left wheel
-    float deltaThetaR = 0.000404 * msg.right / WHEEL_RADIUS_RIGHT; // Calculate rotation angle for right wheel
+    float deltaThetaL = 0.000404 * msg.left*left_p / WHEEL_RADIUS_LEFT; // Calculate rotation angle for left wheel
+    float deltaThetaR = 0.000404 * msg.right*right_p / WHEEL_RADIUS_RIGHT; // Calculate rotation angle for right wheel
 
     // Calculate the linear displacement and change in orientation
     float dS = (WHEEL_RADIUS_LEFT * deltaThetaL + WHEEL_RADIUS_RIGHT * deltaThetaR) / 2;
@@ -138,23 +148,6 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
 }
 
 
-
-void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
-    if(msg.left<0){
-        left_p = -1;
-    }
-    else{
-        left_p = 1;
-    }
-    if(msg.right<0){
-        right_p = -1;
-    }
-    else{
-        right_p = 1;
-    }
-}
-
-
 int main(int argc, char **argv) {
     ros::init(argc, argv, "odometry_new");
     std::string ns_for_group = ros::this_node::getNamespace();