diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp index 8732ade64fa59fb1ebaeacc1f423b2710e18f6eb..f917c59ee9b09e8c4c35e50e85fb4d36f09bc334 100644 --- a/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp +++ b/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp @@ -30,7 +30,7 @@ public: }; -PIDController pid(3, 0, 5); +PIDController pid(30, 0, 5); PIDController pid1(40, 0, 0); PIDController pid_v(1.5, 0, 0); @@ -49,7 +49,7 @@ float ref_w = 0; float w_now = 0; float v_now = 0; -float initial_pwm = 25; +float initial_pwm = 15; //initial 26.5 void controlOperation(const geometry_msgs::Twist& msg) { float flag; @@ -84,7 +84,7 @@ void pending(){ void pureRotation(float flag) { - float rotating_pwm = 15; + float rotating_pwm = 10; float right; float left; if (flag > 0) { diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp index e6952a51c0b691d5721ae4938b9ad0958bac58b3..2a08d16c61914244e19d1ab990e432c0bc996c17 100644 --- a/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp +++ b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp @@ -13,7 +13,7 @@ ros::Time time_previous, time_now; // Timing variables ros::Duration elapsed_time; // Duration to calculate elapsed time // Robot's position and orientation variables -float x = 0.0, y = 0.0, phi = 0.0; +float x = 0.0, y = 0.0, phi = 0.0, S = 0.0; double time_float; // Floating-point representation of elapsed time @@ -76,7 +76,7 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) { // Calculate the linear displacement and change in orientation float dS = (WHEEL_RADIUS_LEFT * deltaThetaL + WHEEL_RADIUS_RIGHT * deltaThetaR) / 2; float dPhi = (WHEEL_RADIUS_RIGHT * deltaThetaR - WHEEL_RADIUS_LEFT * deltaThetaL) / (2 * HALF_WHEEL_BASE); - + S += abs(dS); // Update position and orientation based on movements x += dS * cos(phi + 0.5 * dPhi); y += dS * sin(phi + 0.5 * dPhi); @@ -107,7 +107,7 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) { float angular_velocity = dPhi / time_float; // Log current position and angular velocity - ROS_INFO_STREAM("Current Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity); + ROS_INFO_STREAM("Current Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity << ", Total distance:"<< S);