使用ePuck进行线跟踪和避障[Webots]

问题描述

因此，我试图通过使用E-puck机器人的地面传感器和距离传感器跟踪直线，来编程一种非常简单的防撞行为。机器人程序是用Python编写的，并且模拟程序正在Webots上运行。机器人应沿着直线前进，直到前部距离传感器检测到障碍物，然后转向无障碍方向，然后回到直线。类似于that。因此，我尝试重现相同的行为，但是当机器人接近障碍物时，它会卡住并且不会尝试避免它。当我删除部分代码后的行或避免障碍的部分时，适当的结果正在工作，但同时它们无法正常工作。我看过一些用C编写的代码，但是我不熟悉C编程语言。因为我已经关注了这些教程，所以请不要发送给我看这些教程。我的代码：

"""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)

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）