From f3a2750ffd0ec19f4654d14298d94ac0dd08307d Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Mon, 18 Nov 2024 15:22:16 +0000
Subject: [PATCH] Added the motorcontrol into robobin
---
ros2/src/robobin/launch/robobin_launch.py | 8 ++-
.../src/robobin/robobin/motor_control_node.py | 67 +++++++++++++++++++
2 files changed, 74 insertions(+), 1 deletion(-)
create mode 100644 ros2/src/robobin/robobin/motor_control_node.py
diff --git a/ros2/src/robobin/launch/robobin_launch.py b/ros2/src/robobin/launch/robobin_launch.py
index 15ee4421..ae003ae1 100755
--- a/ros2/src/robobin/launch/robobin_launch.py
+++ b/ros2/src/robobin/launch/robobin_launch.py
@@ -7,10 +7,16 @@ def generate_launch_description():
return LaunchDescription([
Node(
package='robobin',
- executable='api_node', # Name of your first node
+ executable='api_node',
name='api_node',
output='screen'
),
+ Node(
+ package='robobin',
+ executable='motor_control_node',
+ name='motor_control_node',
+ output='screen'
+ ),
# Add additional nodes
# Example:
# Node(
diff --git a/ros2/src/robobin/robobin/motor_control_node.py b/ros2/src/robobin/robobin/motor_control_node.py
new file mode 100644
index 00000000..1ad8ecfc
--- /dev/null
+++ b/ros2/src/robobin/robobin/motor_control_node.py
@@ -0,0 +1,67 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+
+class MotorController(Node):
+ def __init__(self):
+ super().__init__('motor_controller')
+ self.subscription = self.create_subscription(Twist,'cmd_vel',self.cmd_vel_callback,10)
+ self.wheel_radius = 0.05 # meters
+ self.wheel_base = 0.30
+ self.get_logger().info("hello")
+
+
+ def cmd_vel_callback(self, msg):
+ v = msg.linear.x
+ omega = msg.angular.z
+
+ # self.get_logger().info(f"linearx: {v}, angluarz: {omega}")
+
+
+ # v_left = v - (self.wheel_base / 2.0) * omega
+ # v_right = v + (self.wheel_base / 2.0) * omega
+
+ # max_speed = 1.0
+ # # Normalize speeds to -1.0 to 1.0
+ # left_speed = max(min(v_left / max_speed, 1.0), -1.0)
+ # right_speed = max(min(v_right / max_speed, 1.0), -1.0)
+
+ # # Convert to PWM duty cycle (0 to 100)
+ # left_pwm = int((left_speed + 1.0) / 2.0 * 100)
+ # right_pwm = int((right_speed + 1.0) / 2.0 * 100)
+
+ if omega == 1:
+ left_pwm = -50
+ right_pwm = 50
+
+ elif omega == -1:
+ left_pwm = 50
+ right_pwm = -50
+
+ if v == 0.5:
+ left_pwm = 75
+ right_pwm = 75
+
+ elif v == -0.5:
+ left_pwm = -75
+ right_pwm = -75
+
+
+
+ self.get_logger().info(f"Left PWM: {left_pwm}, Right PWM: {right_pwm}")
+
+ # Set motor directions based on sign of speeds
+ # left_direction = 1 if left_speed >= 0 else 0
+ # right_direction = 1 if right_speed >= 0 else 0
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = MotorController()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
--
GitLab