From b194f4d87093f6f07cd6d51e2a9872a5fa09803b Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 17:51:12 +0100 Subject: [PATCH] change control --- src/modelec_com/src/pcb_odo_interface.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index cec2337..a07e29d 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -73,19 +73,22 @@ namespace Modelec "joy", 30, [this](const sensor_msgs::msg::Joy::SharedPtr msg) { - double forward = msg->axes[1]; - double turn = msg->axes[2]; + double left_axis = msg->axes[1]; + double right_axis = msg->axes[2]; - if (fabs(forward) < 0.05) forward = 0.0; - if (fabs(turn) < 0.05) turn = 0.0; + if (fabs(left_axis) < 0.05) left_axis = 0.0; + if (fabs(right_axis) < 0.05) right_axis = 0.0; - int left = static_cast(forward * 626 - turn * 626); - int right = static_cast(forward * 626 + turn * 626); + // int left = static_cast(forward * 626 - turn * 626); + // int right = static_cast(forward * 626 + turn * 626); - left = std::max(-626, std::min(626, left)); - right = std::max(-626, std::min(626, right)); + int left_motor = static_cast(left_axis * 626); + int right_motor = static_cast(right_axis * 626); - SetMotor(left, right); + left_motor = std::max(-626, std::min(626, left_motor)); + right_motor = std::max(-626, std::min(626, right_motor)); + + SetMotor(left_motor, right_motor); }); start_odo_sub_ = this->create_subscription(