机器人仿真笔记[3]-基于Webots的避障机器人

搭建世界

[https://www.guyuehome.com/26706]
添加矩形区域，添加物体

放置机器人

使用预置的e-puck机器人

e-puck是一款小型机器人，带有差速轮，10个LED和几个传感器，其中包括8个DistanceSensors和一个Camera

编写控制器

[https://www.guyuehome.com/27514]

"""my_first_controller controller."""
# -*- coding: UTF-8 -*-
# author：罗伯特祥

from controller import Robot

# 参数定义
sensor_num = 8
speed_max = 6.28
range_ = 1024 / 2
ps_sensor_name = ['ps0','ps1','ps2','ps3','ps4','ps5','ps6','ps7']

#二维上的障碍物矩阵
matrix = []
matrix.append([11, 12, 8, -2, -3, -5, -7, -9])
matrix.append([-9, -8, -5, -1, -2, 6, 12, 11])

def main():
    # 实例化机器人类
    robot = Robot()

    # 获取当前世界的基本时间步长
    timestep = int(robot.getBasicTimeStep())

    # 关联电机设备
    left_motor = robot.getMotor('left wheel motor')
    right_motor = robot.getMotor('right wheel motor')

    # 关联并使能传感器设备
    ps_sensor = []
    for i_ in range(sensor_num):
        ps_sensor.append(robot.getDistanceSensor(ps_sensor_name[i_]))
        ps_sensor[i_].enable(timestep)
    
    # 设置电机运行模式
    left_motor.setPosition(float('inf'))
    right_motor.setPosition(float('inf'))
    
    # 设置差速
    left_motor.setVelocity(3)
    right_motor.setVelocity(3)

    
    speed = [0,0]
    # Main loop:
    while robot.step(timestep) != -1:
        # 获取传感器的值
        ps_sensor_value = []
        for i_ in range(sensor_num):
            ps_sensor_value.append(ps_sensor[i_].getValue())
            print(ps_sensor_name[i_] + " = " + str(ps_sensor_value[i_]))

        for i_ in range(2):
            speed[i_] = 0;
            for j_ in range(sensor_num):
                speed[i_] += matrix[i_][j_] * (1 - (ps_sensor_value[j_] / range_))

        left_motor.setVelocity(0.5 * speed[0])
        right_motor.setVelocity(0.5 * speed[1]) 
        
print("my first controller of the e-puck robot started...")
main()