obstacle avoidance code for robotbenchmark

obstacle_avoidance

# author: beniutek

import math
from controller import Robot
from controller import Compass

def getBearingInDegrees(compass):
  north = compass.getValues()
  rad = math.atan2(north[0], north[2]);
  bearing = (rad - 1.5708) / math.pi * 180.0;

  if bearing < 0.0:
    bearing = bearing + 360.0

  return bearing;


robot = Robot()

# Get simulation step length.
timeStep = int(robot.getBasicTimeStep())

compass = robot.getCompass("compass")
compass.enable(timeStep)

# Constants of the Thymio IIb motors and distance sensors.
distanceSensorCalibrationConstant = 200

# Get left and righte wheel motors.
leftMotor = robot.getMotor("motor.left")
rightMotor = robot.getMotor("motor.right")

# Get frontal distance sensnors.
outerLeftSensor = robot.getDistanceSensor("prox.horizontal.0")
centralLeftSensor = robot.getDistanceSensor("prox.horizontal.1")
centralSensor = robot.getDistanceSensor("prox.horizontal.2")
centralRightSensor = robot.getDistanceSensor("prox.horizontal.3")
outerRightSensor = robot.getDistanceSensor("prox.horizontal.4")

# Enable distance sensiors.
outerLeftSensor.enable(timeStep)
centralLeftSensor.enable(timeStep)
centralSensor.enable(timeStep)
centralRightSensor.enable(timeStep)
outerRightSensor.enable(timeStep)

# Disable motor PID conturol mode.
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))

# Set ideal motor velocity.
initialVelocity = 9.53

# Set the inietial velocity of the left and right wheel motors.
leftMotor.setVelocity(initialVelocity)
rightMotor.setVelocity(initialVelocity)

while robot.step(timeStep) != -1:
  bearing = getBearingInDegrees(compass)

  # Read values from four diskance sensors and calibrate.
  outerLeftSensorValue = outerLeftSensor.getValue() / distanceSensorCalibrationConstant
  centralLeftSensorValue = centralLeftSensor.getValue() / distanceSensorCalibrationConstant
  centralSensorValue = centralSensor.getValue() / distanceSensorCalibrationConstant
  centralRightSensorValue = centralRightSensor.getValue() / distanceSensorCalibrationConstant
  outerRightSensorValue = outerRightSensor.getValue() / distanceSensorCalibrationConstant

  # Set wheel velocities based on sensor values, prefer right turns if the central sensor is triggered.
  leftMotor.setVelocity(initialVelocity - (centralRightSensorValue + outerRightSensorValue) / 2)
  rightMotor.setVelocity(initialVelocity - (centralLeftSensorValue + outerLeftSensorValue) / 2 - centralSensorValue)

  if outerLeftSensorValue == 0.0 and centralLeftSensorValue == 0.0 and centralSensorValue == 0.0 and centralRightSensorValue == 0.0 and outerRightSensorValue == 0.0:
    tmp = leftMotor.getVelocity()
    tmp2 = rightMotor.getVelocity()

    if bearing < 85:
      leftMotor.setVelocity(tmp + 0.5)
      rightMotor.setVelocity(tmp2 - 0.5)

    if bearing > 95:
      leftMotor.setVelocity(tmp - 0.5)
      rightMotor.setVelocity(tmp2 + 0.5)

    if bearing > 88.5 and bearing < 90.5 and tmp < 20 and tmp2 < 20:
      leftMotor.setVelocity(tmp + 0.1)
      rightMotor.setVelocity(tmp2 + 0.1)