From fd42a9d3fd3f805a1a1dae527376ee6ad3a5bcd4 Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Mon, 29 Jul 2024 13:59:12 +0800
Subject: [PATCH] =?UTF-8?q?=E2=80=9C~=E2=80=9D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../src/forklift/control_node.cpp | 2 +-
.../src/forklift/odometry_new.cpp | 39 ++++++++-----------
2 files changed, 17 insertions(+), 24 deletions(-)
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 a82925d6..8732ade6 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(2, 0, 5);
+PIDController pid(3, 0, 5);
PIDController pid1(40, 0, 0);
PIDController pid_v(1.5, 0, 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 13b86b43..e6952a51 100644
--- a/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
+++ b/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();
--
GitLab