如何解决使用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 举报,一经查实,本站将立刻删除。