Pioneer3-DX Robot Simulation: Unexpected Behavior When Combining Turn and Move Functions

45 views Asked by At

I'm working on simulating a Pioneer3-DX robot's movement in a Webots environment using Python. When I call the turn_to_angle and move_distance functions separately, the robot behaves as expected. However, when I attempt to run them together, I observe unexpected behavior in the angle of rotation. It rotates more than 90 degrees.

controller.py

from controller import Robot
import time
import math

# Define robot specifications
MAX_WHEEL_VELOCITY = 6.15  # Adjusted maximal wheel velocity in radians per second
WHEEL_DIAMETER = 0.195  # Wheel diameter in meters
DISTANCE_BETWEEN_WHEELS = 0.325  # Distance between wheels in meters

# Initialize the robot and motors
robot = Robot()
left_motor = robot.getDevice('left wheel')
right_motor = robot.getDevice('right wheel')

# Set the target position (in radians) for both wheels
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))

def move_distance(distance):
    print(f"Moving {distance} meters...")
    # Calculate the time needed to move the specified distance
    time_to_move = distance / (MAX_WHEEL_VELOCITY * WHEEL_DIAMETER / 2)
    # Set the motors to the maximum speed
    left_motor.setVelocity(MAX_WHEEL_VELOCITY)
    right_motor.setVelocity(MAX_WHEEL_VELOCITY)
    # Wait for the robot to move the specified distance
    robot.step(int(time_to_move * 1000))  # Convert time to milliseconds
    # Stop the motors
    left_motor.setVelocity(0)
    right_motor.setVelocity(0)
    print("Movement completed.")

def turn_to_angle(angle_degrees):
    print(f"Turning to {angle_degrees} degrees...")
    # Set the target velocity for both wheels
    velocity = -MAX_WHEEL_VELOCITY  # Set the velocity for the left wheel (negative for right rotation)
    left_motor.setVelocity(velocity)
    right_motor.setVelocity(-velocity)  # Set the velocity for the right wheel (positive for right rotation)

    # Define the parameters of the robot
    wheel_radius = WHEEL_DIAMETER / 2  # Wheel radius (half of wheel diameter)
    wheelbase_width = DISTANCE_BETWEEN_WHEELS  # Distance between wheels

    # Calculate the rotation duration for the specified angle
    rotation_duration = (wheelbase_width * math.pi * angle_degrees) / (360 * wheel_radius * MAX_WHEEL_VELOCITY)

    # Get the current time
    start_time = robot.getTime()

    # Run the robot
    while robot.step(32) != -1:  # Adjust the simulation step size to 32 milliseconds
        current_time = robot.getTime()
        elapsed_time = current_time - start_time

        if elapsed_time >= rotation_duration:
            break

    left_motor.setVelocity(0)  # Stop the left wheel
    right_motor.setVelocity(0)  # Stop the right wheel

    print(f"Turn to {angle_degrees} degrees completed.")



# Turn the robot 90 degrees
turn_to_angle(90)

# Add a delay before moving
# time.sleep(1)  # Add a 1 second delay (you can adjust this as needed)

# Move the robot 1 meter
move_distance(1)

Additional Information: I'm using the Pioneer3-DX robot in my simulation. Here's the link to Pioneer3-DX robot model: Pioneer3-DX Robot Model

What I've Tried:

  • I've extensively tested each function separately, and they work as intended.
  • I've reviewed the code for any logical errors, but I haven't been able to identify the root cause of the issue.
  • Tried adding delay using sleep(). But issue persist.

Request for Assistance:

I'm seeking insights or suggestions on how to ensure consistent behavior when combining these functions. If anyone has experience with the Pioneer3-DX robot simulation or can provide guidance on debugging this issue, I would greatly appreciate it.

0

There are 0 answers