From 1ccf097ac93030a1634773dc966b6cfee7bcf9fd Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Thu, 18 Jul 2024 17:10:49 +0800
Subject: [PATCH] modified cpp file to control the motion of the robot
---
catkin_ws/src/asclinic_pkg/CMakeLists.txt | 26 ++-
.../src/forklift/control_node.cpp | 156 +++++++++++++
.../src/forklift/fcontroller1.cpp | 220 ------------------
3 files changed, 171 insertions(+), 231 deletions(-)
create mode 100644 catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp
delete mode 100644 catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp
diff --git a/catkin_ws/src/asclinic_pkg/CMakeLists.txt b/catkin_ws/src/asclinic_pkg/CMakeLists.txt
index 898d205c..fdac4ef5 100644
--- a/catkin_ws/src/asclinic_pkg/CMakeLists.txt
+++ b/catkin_ws/src/asclinic_pkg/CMakeLists.txt
@@ -17,6 +17,8 @@ find_package(catkin REQUIRED COMPONENTS
cv_bridge
genmsg
roslib
+ nav_msgs
+ geometry_msgs
)
## System dependencies are found with CMake's conventions
@@ -207,17 +209,17 @@ add_executable(i2c_for_sensors src/nodes/i2c_for_sensors.c
add_executable(emulate_i2c_for_motors src/nodes/emulate_i2c_for_motors.cpp)
add_executable(web_interface_connector src/nodes/web_interface_connector.cpp)
add_executable(control_policy_skeleton src/nodes/control_policy_skeleton.cpp)
-add_executable(run_pub_motor_servo_node_LJA src/run_pub_motor_servo_node_LJA.cpp)
-add_executable(recieve_lidar src/recieve_lidar.cpp)
-add_executable(recieve_direction src/recieve_direction.cpp)
-add_executable(recieve_encoder src/recieve_encoder.cpp)
-add_executable(controller src/controller.cpp)
-add_executable(odometry src/odometry.cpp)
-add_executable(controller1 src/controller1.cpp)
-add_executable(Kalman src/Kalman.cpp)
-add_executable(fcontroller1 src/fcontroller1.cpp)
-add_executable(final_controller src/final_controller.cpp)
-
+add_executable(run_pub_motor_servo_node_LJA src/ascr/run_pub_motor_servo_node_LJA.cpp)
+add_executable(recieve_lidar src/ascr/recieve_lidar.cpp)
+add_executable(recieve_direction src/ascr/recieve_direction.cpp)
+add_executable(recieve_encoder src/ascr/recieve_encoder.cpp)
+add_executable(controller src/ascr/controller.cpp)
+add_executable(odometry src/ascr/odometry.cpp)
+add_executable(controller1 src/ascr/controller1.cpp)
+add_executable(Kalman src/ascr/Kalman.cpp)
+add_executable(fcontroller1 src/ascr/fcontroller1.cpp)
+add_executable(final_controller src/ascr/final_controller.cpp)
+add_executable(control_node src/forklift/control_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -252,6 +254,7 @@ add_dependencies(controller1 asclinic_pkg_generate_mes
add_dependencies(Kalman asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(fcontroller1 asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(final_controller asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(control_node asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -283,6 +286,7 @@ target_link_libraries(controller1 ${catkin_LIBRARIES})
target_link_libraries(Kalman ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
target_link_libraries(fcontroller1 ${catkin_LIBRARIES})
target_link_libraries(final_controller ${catkin_LIBRARIES})
+target_link_libraries(control_node ${catkin_LIBRARIES})
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp
new file mode 100644
index 00000000..a82925d6
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp
@@ -0,0 +1,156 @@
+#define _USE_MATH_DEFINES
+#include "ros/ros.h"
+#include <math.h>
+#include <fstream>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <chrono>
+#include "asclinic_pkg/LeftRightFloat32.h"
+#include <std_msgs/UInt32.h>
+
+class PIDController {
+private:
+ float Kp, Ki, Kd;
+ float prev_error;
+ float integral;
+
+public:
+ PIDController(float p, float i, float d) : Kp(p), Ki(i), Kd(d), prev_error(0), integral(0) {}
+
+ float compute(float error, float dt) {
+ float derivative = (error - prev_error) / dt;
+ integral += error * dt; // Proper integration over time
+ float output = Kp * error + Ki * integral + Kd * derivative;
+ prev_error = error;
+ return output;
+ }
+ void resetPrevError() {
+ prev_error = 0;
+ }
+};
+
+
+PIDController pid(2, 0, 5);
+PIDController pid1(40, 0, 0);
+PIDController pid_v(1.5, 0, 0);
+
+
+ros::Subscriber odom_reciever;
+ros::Subscriber velocity_reciever;
+ros::Publisher output_pub;
+
+void update0();
+void pureRotation(float flag);
+void pending();
+
+float time_sep = 0.1;
+float ref_v = 0;
+float ref_w = 0;
+float w_now = 0;
+float v_now = 0;
+
+float initial_pwm = 25;
+//initial 26.5
+void controlOperation(const geometry_msgs::Twist& msg) {
+ float flag;
+
+
+
+
+ if(msg.angular.z!=0){
+ flag = msg.angular.z;
+ pureRotation(flag);
+ }
+ else if(msg.linear.x!=0){
+ update0();
+ }
+ else{
+ pending();
+ }
+}
+
+void pending(){
+ float right;
+ float left;
+
+ right = 0;
+ left = 0;
+ asclinic_pkg::LeftRightFloat32 output_msg;
+ output_msg.left = left;
+ output_msg.right = right;
+ output_pub.publish(output_msg);
+}
+
+void pureRotation(float flag) {
+
+
+ float rotating_pwm = 15;
+ float right;
+ float left;
+ if (flag > 0) {
+ right = rotating_pwm;
+ left = -rotating_pwm;
+ } else {
+ right = -rotating_pwm;
+ left = rotating_pwm;
+ }
+
+ asclinic_pkg::LeftRightFloat32 output_msg;
+ output_msg.left = left;
+ output_msg.right = right;
+ output_pub.publish(output_msg);
+ ROS_INFO_STREAM("Turning");
+}
+
+
+
+void update0() {
+
+
+ //compute the error
+ float error = ref_w - w_now;
+ //calculate the output of control
+ float control_output = pid.compute(error, time_sep);
+
+ float left = initial_pwm - control_output / 2;
+ float right = initial_pwm + control_output / 2;
+ if (right > 95) {
+ right = 90;
+ } else if (right < -95) {
+ right = -90;
+ }
+ if (left > 95) {
+ left = 90;
+ } else if (left < -95) {
+ left = -90;
+ }
+ asclinic_pkg::LeftRightFloat32 output_msg;
+ output_msg.left = left;
+ output_msg.right = right;
+ output_pub.publish(output_msg);
+
+
+
+}
+void odomChange(const nav_msgs::Odometry& Umsg){
+
+ w_now = Umsg.twist.twist.angular.z;
+
+}
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "control_node");
+ std::string ns_for_group = ros::this_node::getNamespace();
+ ros::NodeHandle nh(ns_for_group);
+ ROS_INFO_STREAM("node started");
+
+
+ odom_reciever = nh.subscribe("/odom", 1, odomChange);
+ velocity_reciever = nh.subscribe("cmd_vel", 1, controlOperation);
+
+ output_pub = nh.advertise<asclinic_pkg::LeftRightFloat32>("/asc/set_motor_duty_cycle", 1, false);
+
+ ros::spin();
+
+ return 0;
+}
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp
deleted file mode 100644
index 376db272..00000000
--- a/catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp
+++ /dev/null
@@ -1,220 +0,0 @@
-#define _USE_MATH_DEFINES
-#include "ros/ros.h"
-#include "asclinic_pkg/vw.h"
-#include <math.h>
-#include <fstream>
-#include <thread>
-#include <mutex>
-#include <chrono>
-#include "asclinic_pkg/LeftRightFloat32.h"
-#include "asclinic_pkg/position.h" // Assuming this is your custom message header
-#include "asclinic_pkg/rotation.h"
-#include <std_msgs/UInt32.h>
-
-class PIDController {
-private:
- float Kp, Ki, Kd;
- float prev_error;
- float integral;
-
-public:
- PIDController(float p, float i, float d) : Kp(p), Ki(i), Kd(d), prev_error(0), integral(0) {}
-
- float compute(float error, float dt) {
- float derivative = (error - prev_error) / dt;
- integral += error * dt; // Proper integration over time
- float output = Kp * error + Ki * integral + Kd * derivative;
- prev_error = error;
- return output;
- }
- void resetPrevError() {
- prev_error = 0;
- }
-};
-
-
-PIDController pid(2, 0, 5);
-PIDController pid1(40, 0, 0);
-PIDController pid_v(1.5, 0, 0);
-
-ros::Subscriber vw_sub;
-ros::Subscriber vw_r_sub;
-ros::Subscriber odometry_sub;
-ros::Subscriber position_sub;
-ros::Subscriber kalman_back;
-
-ros::Publisher output_pub;
-ros::Publisher state_pub;
-ros::Publisher kalman_request;
-
-std::mutex mtx;
-
-float current_phi;
-float ref_phi;
-float time_sep = 0.1;
-float ref_v = 0;
-float ref_w = 0;
-float w_now = 0;
-float v_now = 0;
-int state_value = 2; // 0 is forward, 1 is rotate , 2 is pending
-
-float initial_pwm = 25;
-//initial 26.5
-void straightline(const asclinic_pkg::vw& vw_msg) {
-
-
- ref_v = vw_msg.v;
- ref_w = vw_msg.w;
-
-
- if(ref_v==0 && ref_w == 0){
- state_value = 2;
- ROS_INFO_STREAM("stopping");
- asclinic_pkg::LeftRightFloat32 output_msg;
- output_msg.left = 0;
- output_msg.right = 0;
- output_pub.publish(output_msg);
- output_pub.publish(output_msg);
-
-
- }
- else{
- state_value = 0;
- }
-
-}
-
-void purerotation(const asclinic_pkg::rotation& rotation_msg) {
- state_value = 1;
- float flag = rotation_msg.direction;
- ref_phi = rotation_msg.ref_phi;
-
- float rotating_pwm = 15;
- float right;
- float left;
- if (flag > 0) {
- right = rotating_pwm;
- left = -rotating_pwm;
- } else {
- right = -rotating_pwm;
- left = rotating_pwm;
- }
-
- ROS_INFO_STREAM("The right rotation percentage is " << right);
- asclinic_pkg::LeftRightFloat32 output_msg;
- output_msg.left = left;
- output_msg.right = right;
- output_pub.publish(output_msg);
- ROS_INFO_STREAM("First message of turning sent");
-
-
-
- // Main rotation loop
- bool condition = true;
- while (condition) {
- float current_phi_value;
- {
- std::lock_guard<std::mutex> lock(mtx);
- current_phi_value = current_phi;
- }
-
- float error = std::abs(ref_phi - current_phi_value);
- ROS_INFO_STREAM("Current phi: " << current_phi_value << ", Ref phi: " << ref_phi << ", Error: " << error);
-
- if (error <= M_PI / 8) {
- condition = false;
- ROS_INFO("Reached desired rotation angle. Stopping rotation.");
-
- // Publish PWM signals to stop rotation
- output_msg.left = 0;
- output_msg.right = 0;
- output_pub.publish(output_msg);
- }
-
- ros::spinOnce();
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
-
- // Ensure motors are stopped
- output_msg.left = 0;
- output_msg.right = 0;
- output_pub.publish(output_msg);
- ROS_INFO_STREAM("Motors stopped after rotation");
-
- std_msgs::UInt32 Umsg;
- Umsg.data = 1;
- kalman_request.publish(Umsg);
-
-
-
-}
-
-void kalman_change(const std_msgs::UInt32& Umsg){
-
- // Publish state message
- std_msgs::UInt32 state_msg;
- state_msg.data = 1;
- state_pub.publish(state_msg);
- ROS_INFO_STREAM("State message published");
-}
-
-void updateO(const asclinic_pkg::vw& w_msg) {
- w_now = w_msg.w;
- v_now = w_msg.v;
-
- if (state_value != 0){
- return;
- }
-
- //compute the error
- float error = ref_w - w_now;
- float error_v = ref_v - v_now;
- //calculate the output of control
- float control_output = pid.compute(error, time_sep);
- float control_output_v = pid_v.compute(error_v, time_sep);
-
- float left = initial_pwm - control_output / 2;
- float right = initial_pwm + control_output / 2;
- if (right > 95) {
- right = 90;
- } else if (right < -95) {
- right = -90;
- }
- if (left > 95) {
- left = 90;
- } else if (left < -95) {
- left = -90;
- }
- asclinic_pkg::LeftRightFloat32 output_msg;
- output_msg.left = left;
- output_msg.right = right;
- output_pub.publish(output_msg);
-
-
-
-}
-void updatep(const asclinic_pkg::position& msg){
- std::lock_guard<std::mutex> lock(mtx);
- current_phi = msg.phi;
-
-}
-
-int main(int argc, char** argv) {
- ros::init(argc, argv, "fcontroller1");
- std::string ns_for_group = ros::this_node::getNamespace();
- ros::NodeHandle nh(ns_for_group);
- ROS_INFO_STREAM("node started");
-
- vw_sub = nh.subscribe("/vw_information", 1, straightline);
- vw_r_sub = nh.subscribe("/vw_r_information", 3, purerotation);
- odometry_sub = nh.subscribe("/angular_velocity_topic", 1, updateO);
- position_sub = nh.subscribe("/current_position", 1, updatep);
- kalman_back = nh.subscribe("/kalman_info", 1, kalman_change);
-
- output_pub = nh.advertise<asclinic_pkg::LeftRightFloat32>("/asc/set_motor_duty_cycle", 1, false);
- state_pub = nh.advertise<std_msgs::UInt32>("/controller1_state", 3, false);
- kalman_request = nh.advertise<std_msgs::UInt32>("/kalman_demand", 1, false);
- ros::spin();
-
- return 0;
-}
--
GitLab