Added the motorcontrol into robobin

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(

ros2/src/robobin/robobin/motor_control_node.py

+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()