# Dr. Khaled Eskaf
#Robotics WorkShop - 1
#06-2024


from controller import Robot, Motor, DistanceSensor

# Define constants
TIME_STEP = 64  # Time step for the simulation
MAX_SPEED = 6.28  # Maximum speed for the motors in radians/second

# Create the Robot instance
robot = Robot()

# Get the motors and set their positions to infinity (to control speed)
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))

# Get the distance sensor and enable it
distanceSensor = robot.getDevice('ps0')  # Assuming 'ps0' is the correct sensor name
distanceSensor.enable(TIME_STEP)

# Set the initial speed of the motors to 10% of the maximum speed
leftMotor.setVelocity(0.1 * MAX_SPEED)
rightMotor.setVelocity(0.1 * MAX_SPEED)

# Main loop
while robot.step(TIME_STEP) != -1:
    # Read the distance sensor value
    distance = distanceSensor.getValue()
    print("Distance sensor reading:", distance)  # Print the sensor reading
    
    # Check if an obstacle is detected
    if distance < 100:  # You can adjust this threshold based on your observations
        # If an obstacle is detected, stop the robot
        leftMotor.setVelocity(0)
        rightMotor.setVelocity(0)
    else:
        # If no obstacle is detected, keep moving forward
        leftMotor.setVelocity(0.1 * MAX_SPEED)
        rightMotor.setVelocity(0.1 * MAX_SPEED)