diff --git a/ros2/src/robobin/robobin/motor_control_node.py b/ros2/src/robobin/robobin/motor_control_node.py index 19619640d91ecc49b779f5d53a01f753766c8287..d049b21ffde3121eece732642070cb227fa03aa8 100644 --- a/ros2/src/robobin/robobin/motor_control_node.py +++ b/ros2/src/robobin/robobin/motor_control_node.py @@ -25,8 +25,8 @@ class MotorController(Node): self.get_logger().info("hello") def cmd_vel_callback(self, msg): - v = msg.linear.x - omega = msg.angular.z + linear_x = msg.linear.x + angular_z = msg.angular.z # self.get_logger().info(f"linearx: {v}, angluarz: {omega}") @@ -43,26 +43,30 @@ class MotorController(Node): # left_pwm = int((left_speed + 1.0) / 2.0 * 100) # right_pwm = int((right_speed + 1.0) / 2.0 * 100) - if omega == 1: - self.left_pwm = -50 - self.right_pwm = 50 - elif omega == -1: - self.left_pwm = 50 - self.right_pwm = -50 - # Handle linear velocity commands - if v == 0.5: + if linear_x == 0.5 and angular_z == 0: + self.left_pwm = 75 self.right_pwm = 75 - elif v == -0.5: + elif linear_x == -0.5 and angular_z == 0: + self.left_pwm = -75 self.right_pwm = -75 + elif linear_x == 0 and angular_z == 0.5: - # STOP command - if v == 0 and omega == 0: + self.left_pwm = -50 + self.right_pwm = 50 + elif linear_x == 0 and angular_z == -0.5: + # Turn right + self.left_pwm = 50 + self.right_pwm = -50 + elif linear_x == 0 and angular_z == 0: + # STOP self.left_pwm = 0 self.right_pwm = 0 self.get_logger().info("STOP command received. Motors stopped.") + + # Log the computed PWM values self.get_logger().info(f"Left PWM: {self.left_pwm}, Right PWM: {self.right_pwm}") # Set motor directions based on sign of speeds