Update motor_control_node.py

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