Added the new motor controller into the ros2 package "robobin".

ros2/src/robobin/robobin/motor_control_node.py

+#!/usr/bin/env python3
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
+from gpiozero import PWMOutputDevice
+from time import sleep
 
-class MotorController(Node):
+class Motor:
+    def __init__(self, node, EnaA, In1A, In2A, EnaB, In1B, In2B):
+        self.node = node
+        self.pwmA = PWMOutputDevice(EnaA)
+        self.in1A = PWMOutputDevice(In1A)
+        self.in2A = PWMOutputDevice(In2A)
+        self.pwmB = PWMOutputDevice(EnaB)
+        self.in1B = PWMOutputDevice(In1B)
+        self.in2B = PWMOutputDevice(In2B)
+
+    def move(self, speed=0.0, turn=0.0):
+        speed = max(-1, min(1, speed))
+        turn = max(-1, min(1, turn))
+
+        leftSpeed = speed - turn
+        rightSpeed = speed + turn
+        '''
+        left_speed = self.desired_speed - (turn_rate * self.motor.wheel_base / 2)
+        right_speed = self.desired_speed + (turn_rate * self.motor.wheel_base / 2)
+        '''
+
+        leftSpeed = max(-1, min(1, leftSpeed))
+        rightSpeed = max(-1, min(1, rightSpeed))
+
+        self.pwmA.value = abs(leftSpeed)
+        self.in1A.value = leftSpeed <=  0
+        self.in2A.value = leftSpeed > 0
+
+        self.pwmB.value = abs(rightSpeed)
+        self.in1B.value = rightSpeed > 0
+        self.in2B.value = rightSpeed <= 0
+
+        self.node.get_logger().info(f"Left Motor: Speed={leftSpeed}, Right Motor: Speed={rightSpeed}")
+
+
+    def stop(self):
+        self.pwmA.value = 0
+        self.pwmB.value = 0
+
+class MotorControlNode(Node):
     def __init__(self):
-        super().__init__('motor_controller')
-        self.get_logger().info("Motor Controller has been started.")
-        
- 
+        super().__init__('motor_control_node')
+        self.get_logger().info("hello")
+        self.motor = Motor(self, 14, 15, 18, 17, 22, 27)
         self.subscription = self.create_subscription(
             Twist,
             'cmd_vel',
@@ -15,76 +56,18 @@ class MotorController(Node):
             10
         )
 
-        self.wheel_radius = 0.05  
-        self.wheel_base = 0.30  
-
-    
-        self.left_pwm = 0
-        self.right_pwm = 0
-
-        self.get_logger().info("hello")
-    
     def cmd_vel_callback(self, msg):
         linear_x = msg.linear.x
         angular_z = 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 linear_x == 0.5 and angular_z == 0:
-  
-            self.left_pwm = 75
-            self.right_pwm = 75
-        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:
-
-            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
-        # left_direction = 1 if left_speed >= 0 else 0
-        # right_direction = 1 if right_speed >= 0 else 0
-
-
+        self.motor.move(speed=linear_x, turn=angular_z)
 
 def main(args=None):
     rclpy.init(args=args)
-    node = MotorController()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        node.shutdown()
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
+    node = MotorControlNode()
+    rclpy.spin(node)
+    rclpy.shutdown()
 
 if __name__ == '__main__':
     main()
+
+#colcon build --symlink-install
\ No newline at end of file