gistfile1.txt

"""Square-path controller for the Webots BB-8 robot."""

from controller import Node, Robot


def usage():
    print("Sample controller of the Sphero's BB-8 robot.")
    print()
    print("The robot follows a square path continuously:")
    print("- Move forward for 10 seconds")
    print("- Turn in place for a fixed duration")
    print("- Repeat")


def main():
    robot = Robot()
    usage()

    time_step = int(robot.getBasicTimeStep())

    # Init motors in velocity-control mode.
    body_yaw_motor = robot.getDevice("body yaw motor")
    body_yaw_motor.setPosition(float("inf"))
    body_yaw_motor.setVelocity(0.0)

    body_pitch_motor = robot.getDevice("body pitch motor")
    body_pitch_motor.setPosition(float("inf"))
    body_pitch_motor.setVelocity(0.0)

    head_yaw_motor = robot.getDevice("head yaw motor")
    head_yaw_motor.setPosition(float("inf"))
    head_yaw_motor.setVelocity(0.0)

    # Enable any camera device present on the robot.
    for i in range(robot.getNumberOfDevices()):
        device = robot.getDeviceByIndex(i)
        if hasattr(device, "getNodeType") and device.getNodeType() == Node.CAMERA:
            device.enable(time_step)

    forward_duration = 10.0
    turn_duration = 1.6
    forward_speed = 4.0
    turn_speed = 2.0

    while robot.step(time_step) != -1:
        t = robot.getTime()
        cycle_time = forward_duration + turn_duration
        phase_time = t % cycle_time

        if phase_time < forward_duration:
            # Drive straight.
            pitch_speed = forward_speed
            yaw_speed = 0.0
        else:
            # Turn in place to approximate a 90-degree corner.
            pitch_speed = 0.0
            yaw_speed = turn_speed

        body_yaw_motor.setVelocity(yaw_speed)
        head_yaw_motor.setVelocity(yaw_speed)
        body_pitch_motor.setVelocity(pitch_speed)


if __name__ == "__main__":
    main()