webots epuck line follower code

“””line_following_behavior controller.”””

from controller import Robot, DistanceSensor, Motor

import numpy as np
#——————————————————-

# Initialize variables
TIME_STEP = 64

MAX_SPEED = 6.28
speed = 1 * MAX_SPEED
# create the Robot instance.

robot = Robot()
# get the time step of the current world.

timestep = int(robot.getBasicTimeStep())   # [ms]
# states

states = [‘forward’, ‘turn_right’, ‘turn_left’]

current_state = states[0]
# counter: used to maintain an active state for a number of cycles

counter = 0

counter_max = 5
#——————————————————-

# Initialize devices
# distance sensors

ps = []

psNames = [‘ps0’, ‘ps1’, ‘ps2’, ‘ps3’, ‘ps4’, ‘ps5’, ‘ps6’, ‘ps7’]

for i in range(8):

    ps.append(robot.getDistanceSensor(psNames[i]))

    ps[i].enable(timestep)
# ground sensors

gs = []

gsNames = [‘gs0’, ‘gs1’, ‘gs2’]

for i in range(3):

    gs.append(robot.getDistanceSensor(gsNames[i]))

    gs[i].enable(timestep)
# motors

leftMotor = robot.getMotor(‘left wheel motor’)

rightMotor = robot.getMotor(‘right wheel motor’)

leftMotor.setPosition(float(‘inf’))

rightMotor.setPosition(float(‘inf’))

leftMotor.setVelocity(0.0)

rightMotor.setVelocity(0.0)
#——————————————————-

# Main loop:

# – perform simulation steps until Webots is stopping the controller

while robot.step(timestep) != -1:

    # Update sensor readings

    psValues = []

    for i in range(8):

        psValues.append(ps[i].getValue())
    gsValues = []

    for i in range(3):

        gsValues.append(gs[i].getValue())
    # detect obstacles

    right_obstacle = psValues[0] > 80.0 or psValues[1] > 80.0 or psValues[2] > 80.0

    left_obstacle = psValues[5] > 80.0 or psValues[6] > 80.0 or psValues[7] > 80.0
    # initialize motor speeds at 50% of MAX_SPEED.

    leftSpeed  = speed

    rightSpeed = speed

    # modify speeds according to obstacles

    if left_obstacle:

        # turn right

        leftSpeed  = speed

        rightSpeed = -speed

    elif right_obstacle:

        # turn left

        leftSpeed  = -speed

        rightSpeed = speed
    # Process sensor data

    line_right = gsValues[0] > 600

    line_left = gsValues[2] > 600
    # Implement the line-following state machine

    if current_state == ‘forward’:

        # Action for the current state

        leftSpeed = speed

        rightSpeed = speed

        # update current state if necessary

        if line_right and not line_left:

            current_state = ‘turn_right’

            counter = 0

        elif line_left and not line_right:

            current_state = ‘turn_left’

            counter = 0
    if current_state == ‘turn_right’:

        # Action for the current state

        leftSpeed = 0.8 * speed

        rightSpeed = 0.4 * speed

        # update current state if necessary

        if counter == counter_max:

            current_state = ‘forward’
    if current_state == ‘turn_left’:

        # Action for the current state

        leftSpeed = 0.4 * speed

        rightSpeed = 0.8 * speed

        # update current state if necessary

        if counter == counter_max:

            current_state = ‘forward’        
    # increment counter

    counter += 1
    #print(‘Counter: ‘+ str(counter), gsValues[0], gsValues[1], gsValues[2])

    print(‘Counter: ‘+ str(counter) + ‘. Current state: ‘ + current_state)
    # Update reference velocities for the motors

    leftMotor.setVelocity(leftSpeed)

    rightMotor.setVelocity(rightSpeed)