def get_robot_device(robot: Robot, name: str, kind: Type[TDevice]) -> TDevice:
device: Optional[Device] = None
try:
# webots 2020b is buggy and always raises TypeError when passed a str,
# however we're aiming to be forwards compatible with 2021a, so try this first.
device = robot.getDevice(name)
except TypeError:
pass
if device is None: # webots 2020b always returns None when not raising TypeError
if kind is Emitter:
device = robot.getEmitter(name)
elif kind is Receiver:
device = robot.getReceiver(name)
elif kind is Motor:
device = robot.getMotor(name)
elif kind is LED:
device = robot.getLED(name)
elif kind is DistanceSensor:
device = robot.getDistanceSensor(name)
elif kind is TouchSensor:
device = robot.getTouchSensor(name)
elif kind is Compass:
device = robot.getCompass(name)
elif kind is Display:
device = robot.getDisplay(name)
if not isinstance(device, kind):
raise TypeError
return device
swamp_colour = b'R\x89\xa7\xff'
swamp_colour2 = b'R\x89\xa6\xff'
left_motor = robot.getMotor("left wheel motor")
right_motor = robot.getMotor("right wheel motor")
left_encoder = left_motor.getPositionSensor()
right_encoder = right_motor.getPositionSensor()
left_heat_sensor = robot.getLightSensor("left_heat_sensor")
right_heat_sensor = robot.getLightSensor("right_heat_sensor")
gyro = robot.getGyro("gyro")
emitter = robot.getEmitter("emitter")
front_sensor_l = robot.getDistanceSensor("ps7")
front_sensor_r = robot.getDistanceSensor("ps0")
left_sensor = robot.getDistanceSensor("ps5")
right_sensor = robot.getDistanceSensor("ps2")
back_sensor_l = robot.getDistanceSensor("ps3")
back_sensor_r = robot.getDistanceSensor("ps4")
color = robot.getCamera("colour_sensor")
cam = robot.getCamera("camera_centre")
cam_right = robot.getCamera("camera_right")
cam_left = robot.getCamera("camera_left")
# get the time step of the current world.
robot = Robot()
timestep = int(robot.getBasicTimeStep())
print ('Move forward and avoid obstabcles.')
leftMotor = robot.getMotor('left motor')
robot.getLED('left motor led').set(0xFFFF00)
leftMotor.setPosition(float('inf')) # Velocity control mode.
rightMotor = robot.getMotor('right motor')
robot.getLED('right motor led').set(0x00FFFF)
rightMotor.setPosition(float('inf')) # Velocity control mode.
maxSpeed = leftMotor.getMaxVelocity()
cruisingSpeed = 0.5 * maxSpeed
leftDistanceSensor = robot.getDistanceSensor('left distance sensor')
leftDistanceSensor.enable(timestep)
robot.getLED('left distance sensor led').set(0xFF0000)
rightDistanceSensor = robot.getDistanceSensor('right distance sensor')
rightDistanceSensor.enable(timestep)
robot.getLED('right distance sensor led').set(0x00FF00)
while robot.step(timestep) != -1:
# Invert the TinkerbotsDistanceSensor.lookupTable to have an appoximation of the distance in meters.
rightDistance = 128 - rightDistanceSensor.getValue()
leftDistance = 128 - leftDistanceSensor.getValue()
delta = leftDistance - rightDistance
leftSpeed = -cruisingSpeed - delta + 0.5
rightSpeed = cruisingSpeed - delta + 0.5
leftSpeed = max(-maxSpeed, min(maxSpeed, leftSpeed))
rightSpeed = max(-maxSpeed, min(maxSpeed, rightSpeed))
rightMotorBack = robot.getMotor('back right wheel')
leftMotorFront.setPosition(float('inf'))
rightMotorFront.setPosition(float('inf'))
leftMotorBack.setPosition(float('inf'))
rightMotorBack.setPosition(float('inf'))
# initialize devices
ps = []
psNames = [
'so0', 'so1', 'so2', 'so3',
'so4', 'so5', 'so6', 'so7'
]
for i in range(8):
ps.append(robot.getDistanceSensor(psNames[i]))
ps[i].enable(TIME_STEP)
while robot.step(TIME_STEP) != -1:
# read sensors outputs
psValues = []
for i in range(8):
psValues.append(ps[i].getValue())
# print(psValues[i])
# detect obstacles
right_obstacle = psValues[0] > 70.0 or psValues[1] > 70.0 or psValues[2] > 70.0
left_obstacle = psValues[5] > 70.0 or psValues[6] > 70.0 or psValues[7] > 70.0
front_obstacle = psValues[3] > 50.0 or psValues[4] > 50.0
# print(right_obstacle)
def __init__(self, webot: Robot, sensor_name: str) -> None:
self.webot_sensor = webot.getDistanceSensor(sensor_name)
self.webot_sensor.enable(int(webot.getBasicTimeStep()))
LShoulderPitch = robot.getMotor("LShoulderPitch")
LShoulderRoll = robot.getMotor("LShoulderRoll")
LElbowRoll = robot.getMotor("LElbowRoll")
# get and enable position sensors
LShoulderPitchS = robot.getPositionSensor("LShoulderPitchS")
LShoulderRollS = robot.getPositionSensor("LShoulderRollS")
LElbowRollS = robot.getPositionSensor("LElbowRollS")
LShoulderPitchS.enable(timestep)
LShoulderRollS.enable(timestep)
LElbowRollS.enable(timestep)
# get and enable sonars
SonarR = robot.getDistanceSensor("Sonar/Right")
SonarL = robot.getDistanceSensor("Sonar/Left")
SonarR.enable(timestep)
SonarL.enable(timestep)
#test movement
LShoulderPitch.setVelocity(1.0)
LShoulderRoll.setVelocity(1.0)
LElbowRoll.setVelocity(1.0)
def getPosition():
return [round(LShoulderPitchS.getValue(),2),round(LShoulderRollS.getValue(),2), round(LElbowRollS.getValue(),2)]
# create the Robot instance.
robot = Robot()
# See Thymio specific Webots names: https://cyberbotics.com/doc/guide/thymio2#thymio2-wbt
# See "robot" specific functions: https://cyberbotics.com/doc/reference/robot?tab=python#getlightsensor
# Motors
leftMotor = robot.getMotor('motor.left')
rightMotor = robot.getMotor('motor.right')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0)
rightMotor.setVelocity(0)
# see functions of DistanceSensor: https://cyberbotics.com/doc/reference/distancesensor#wb_distance_sensor_enable
# sensors
sens0 = robot.getDistanceSensor("prox.horizontal.0")
sens1 = robot.getDistanceSensor("prox.horizontal.1")
sens2 = robot.getDistanceSensor("prox.horizontal.2")
sens3 = robot.getDistanceSensor("prox.horizontal.3")
sens4 = robot.getDistanceSensor("prox.horizontal.4")
# enable distance sensor measurements in sampling period of TIME_STEP
sens0.enable(TIME_STEP)
sens1.enable(TIME_STEP)
sens2.enable(TIME_STEP)
sens3.enable(TIME_STEP)
sens4.enable(TIME_STEP)
# Wall following algorithm
while robot.step(TIME_STEP) != -1:
# get the values read by the sensor
import struct
# Create a robot instance
robot = Robot()
# Declare constants
TIMESTEP = int(robot.getBasicTimeStep())
COMM_CHANNEL = 14
ROBOT_SPEED = 5.0
# Sensors setup
ds = []
dsNames = ['ds0', 'ds1']
for name in dsNames:
ds.append(robot.getDistanceSensor(name))
for i in range(2):
ds[i].enable(TIMESTEP)
# Motors setup
wheels = []
wheelsNames = ['left wheel motor', 'right wheel motor']
for name in wheelsNames:
wheels.append(robot.getMotor(name))
for i in range(2):
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
# Emitter setup
# get the time step of the current world.
timestep = 32
max_speed = 7
#Created motor instances
left_motor = robot.getDevice('motor_1')
right_motor = robot.getDevice('motor_2')
left_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setPosition(float('inf'))
right_motor.setVelocity(0.0)
left_ir = robot.getDistanceSensor('ir_1')
left_ir.enable(timestep)
right_ir = robot.getDistanceSensor('ir_2')
right_ir.enable(timestep)
centre_ir = robot.getDistanceSensor('ir_3')
centre_ir.enable(timestep)
r_ir = robot.getDistanceSensor('ir_4')
r_ir.enable(timestep)
l_ir = robot.getDistanceSensor('ir_5')
l_ir.enable(timestep)
count = 0
while robot.step(timestep) != -1:
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# led = robot.getLED('ledname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
emitter = robot.getEmitter("emitter")
sensorFLL = robot.getDistanceSensor("FLL")
sensorFL = robot.getDistanceSensor("FL")
sensorFR = robot.getDistanceSensor("FR")
sensorFRR = robot.getDistanceSensor("FRR")
lidar = robot.getLidar("lidar")
sensorFLL.enable(timestep)
sensorFL.enable(timestep)
sensorFR.enable(timestep)
sensorFRR.enable(timestep)
leftWheel = robot.getMotor("left_motor")
rightWheel = robot.getMotor("right_motor")
leftOdo = robot.getPositionSensor("left_odo")
# create the Robot instance.
robot = Robot()
# get the wheels
wheels = []
wheelNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for name in wheelNames:
motor = robot.getMotor(name)
motor.setPosition(float('inf'))
motor.setVelocity(-1.5)
wheels.append(motor)
ds = []
dsNames = ['ds_left', 'ds_right']
for name in dsNames:
sensor = robot.getDistanceSensor(name)
sensor.enable(TIME_STEP)
ds.append(sensor)
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
def goForward():
for motor in wheels:
motor.setVelocity(-3)
from controller import Robot
import os
def callback(data):
global velocity
global message
message = 'Received velocity value: ' + str(data.data)
velocity = data.data
robot = Robot()
timeStep = int(robot.getBasicTimeStep())
left = robot.getMotor('motor.left')
right = robot.getMotor('motor.right')
sensor = robot.getDistanceSensor(
'prox.horizontal.2') # front central proximity sensor
sensor.enable(timeStep)
left.setPosition(float('inf')) # turn on velocity control for both motors
right.setPosition(float('inf'))
velocity = 0
left.setVelocity(velocity)
right.setVelocity(velocity)
message = ''
print('Initializing ROS: connecting to ' + os.environ['ROS_MASTER_URI'])
robot.step(timeStep)
rospy.init_node('listener', anonymous=True)
print('Subscribing to "motor" topic')
robot.step(timeStep)
rospy.Subscriber('motor', Float64, callback)
pub = rospy.Publisher('sensor', Float64, queue_size=10)
print('Running the control loop')
timeStep = int(robot.getBasicTimeStep())
# Constants of the e-puck motors and distance sensors.
maxMotorVelocity = 4.2
num_left_dist_sensors = 4
num_right_dist_sensors = 4
right_threshold = [75, 75, 75, 75]
left_threshold = [75, 75, 75, 75]
# Get left and right wheel motors.
leftMotor = robot.getMotor("left wheel motor")
rightMotor = robot.getMotor("right wheel motor")
# Get frontal distance sensors.
dist_left_sensors = [
robot.getDistanceSensor('ps' + str(x))
for x in range(num_left_dist_sensors)
] # distance sensors
list(map((lambda s: s.enable(timeStep)),
dist_left_sensors)) # Enable all distance sensors
dist_right_sensors = [
robot.getDistanceSensor('ps' + str(x))
for x in range(num_right_dist_sensors, 8)
] # distance sensors
list(map((lambda t: t.enable(timeStep)),
dist_right_sensors)) # Enable all distance sensors
# Disable motor PID control mode.
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
class ScribbleBot:
def __init__(self):
self.robot = Robot()
# Define and enable distance sensors
self.ds = [
self.robot.getDistanceSensor(name)
for name in ['ds_left', 'ds_right']
]
for sensor in self.ds:
sensor.enable(TIME_STEP)
# Define motors
self.front_left_motor = self.robot.getMotor('wheel1')
self.front_right_motor = self.robot.getMotor('wheel2')
self.back_left_motor = self.robot.getMotor('wheel3')
self.back_right_motor = self.robot.getMotor('wheel4')
# Group motors for easy access
self.motors = [
self.front_left_motor, self.front_right_motor,
self.back_left_motor, self.back_right_motor
]
# Enable motors
for motor in self.motors:
motor.setPosition(float('inf'))
motor.setVelocity(0.0)
# Define encoders
self.enc_front_left = self.robot.getPositionSensor(
'front left position')
self.enc_front_right = self.robot.getPositionSensor(
'front right position')
self.enc_back_left = self.robot.getPositionSensor('back left position')
self.enc_back_right = self.robot.getPositionSensor(
'back right position')
# Enable encoders
self.enc_front_left.enable(TIME_STEP)
self.enc_front_right.enable(TIME_STEP)
self.enc_back_left.enable(TIME_STEP)
self.enc_back_right.enable(TIME_STEP)
# Line generator
self.pen = self.robot.getPen('pen')
self.pen.write(True)
# Positions of encoders
self.position_front_left = 0
self.position_back_left = 0
self.position_front_right = 0
self.position_back_right = 0
# Internal velocities, one per side
self.left_vel = 0
self.right_vel = 0
# List for distance sensor values
self.ds_values = []
def write_motors(self):
"""Write the current velocities into motor objects"""
self.front_left_motor.setVelocity(self.left_vel)
self.back_left_motor.setVelocity(self.left_vel)
self.front_right_motor.setVelocity(self.right_vel)
self.back_right_motor.setVelocity(self.right_vel)
def stop_motors(self):
"""Helper util to stop motors"""
self.left_vel = 0
self.right_vel = 0
self.write_motors()
def capture_position(self):
"""Capture current encoder values in position variables"""
# Left positions
self.position_front_left = self.enc_front_left.getValue()
self.position_back_left = self.enc_back_left.getValue()
# Right positions
self.position_front_right = self.enc_front_right.getValue()
self.position_back_right = self.enc_back_right.getValue()
def check_left_moved(self, distance: float) -> bool:
"""
Manages a left side distance move. If either motor has achieved
distance required, it is manually stopped. If both have, function
returns `True`
"""
front_moved = abs(self.enc_front_left.getValue() -
self.position_front_left) > distance
back_moved = abs(self.enc_back_left.getValue() -
self.position_back_left) > distance
if front_moved:
self.front_left_motor.setVelocity(0)
if back_moved:
self.back_left_motor.setVelocity(0)
both = front_moved and back_moved
if both:
self.left_vel = 0
return both
def check_right_moved(self, distance: float) -> bool:
"""
Manages a right side distance move. If either motor has achieved
distance required, it is manually stopped. If both have, function
returns `True`
"""
front_moved = abs(self.enc_front_right.getValue() -
self.position_front_right) > distance
back_moved = abs(self.enc_back_right.getValue() -
self.position_back_right) > distance
if front_moved:
self.front_right_motor.setVelocity(0)
if back_moved:
self.back_right_motor.setVelocity(0)
both = front_moved and back_moved
if both:
self.right_vel = 0
return both
def make_turn(self, left_dist: float, right_dist: float):
"""
Makes a turn based on velocities and encoder positions. Blocking.
Velocities must be set outside function call. Each wheel is moved
to corresponding side distance and stopped
"""
self.write_motors()
self.capture_position()
while self.robot.step(TIME_STEP) != -1:
left_moved = self.check_left_moved(left_dist)
right_moved = self.check_right_moved(right_dist)
if left_moved and right_moved:
break
def make_left_turn(self, distance: float):
"""
Make a left turn by a distance. Blocking.
Velocities must be set outside function call. `distance`
pertains to right wheel, left wheel distance is calculated from this
and velocity ratio for perfect arcing turn
"""
ratio = self.left_vel / self.right_vel
self.make_turn(ratio * distance, distance)
def make_right_turn(self, distance: float):
"""
Make a right turn by a distance. Blocking.
Velocities must be set outside function call. `distance`
pertains to left wheel, right wheel distance is calculated from this
and velocity ratio for perfect arcing turn
"""
ratio = self.right_vel / self.left_vel
self.make_turn(distance, ratio * distance)
def move_dist(self, distance: float):
"""
Move a distance forward. Blocking.
Velocities must be set outside function call
"""
self.write_motors()
self.capture_position()
while self.robot.step(TIME_STEP) != -1:
left_moved = self.check_left_moved(distance)
right_moved = self.check_right_moved(distance)
if left_moved and right_moved:
break
def load_distance_values(self):
"""Load current distance values from `self.ds` into `self.ds_values`"""
self.ds_values = []
for ds in self.ds:
self.ds_values.append(ds.getValue())
def init_sensors(self):
"""Initialize sensors to avoid error values like NaN"""
for i in range(0, 5):
if self.robot.step(TIME_STEP) == -1:
break
for ds in self.ds:
ds.getValue()
self.enc_front_left.getValue()
self.enc_front_right.getValue()
self.enc_back_left.getValue()
self.enc_back_right.getValue()
def do_the_wave(self):
"""Do the waaaaave"""
# Entry turn to the right
self.left_vel = 3
self.right_vel = 0.1
self.make_right_turn(6)
# Arcing main turn to the left
self.left_vel = 1
self.right_vel = 3.5
self.make_left_turn(15.8)
# Exiting turn to the right
self.left_vel = 3
self.right_vel = 0.5
self.make_right_turn(4.5)
# Aligning turn to the right, slow and precise
self.left_vel = 1
self.right_vel = 0.2
self.make_right_turn(1.94)
def search_for_wall(self):
"""Drive forward until wall is found"""
print('Finding end wall...')
self.left_vel = 3
self.right_vel = 3
self.write_motors()
while self.robot.step(TIME_STEP) != -1:
self.load_distance_values()
# print('FL: {} FR: {}'.format(self.ds_values[DS_L], self.ds_values[DS_R]))
if self.ds_values[DS_L] < DS_WALL_STOP and self.ds_values[
DS_R] < DS_WALL_STOP:
break
def setup(self):
"""Pre-movement initializations. Runs once"""
print('Initializing...')
self.init_sensors()
self.left_vel = 3
self.right_vel = 3
self.write_motors()
def main(self):
"""Main code. Usually a loop or series of loops"""
print('Running...')
self.move_dist(7)
for i in range(4):
self.do_the_wave()
self.search_for_wall()
def cleanup(self):
"""Post-movement finishing code"""
self.stop_motors()
print('Finished drawing.')
def run(self):
"""Primary running method"""
self.setup()
self.main()
self.cleanup()
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
#Инициализация ИК датчиков
IRSensorNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
IRSensors = []
for sensor in IRSensorNames:
IRSensors.append(robot.getDistanceSensor(sensor))
#----------
#Инициализация датчиков линии
GroundSensorNames = ['gs0', 'gs1', 'gs2']
GroundSensors = []
for sensor in GroundSensorNames:
GroundSensors.append(robot.getDistanceSensor(sensor))
#----------
#включаем все сенсоры
for sensor in GroundSensors:
sensor.enable(TIME_STEP)
for sensor in IRSensors:
sensor.enable(TIME_STEP)
#----------
# create the Robot instance.
robot = Robot()
#See Thymio specific Webots names: https://cyberbotics.com/doc/guide/thymio2#thymio2-wbt
#See "robot" specific functions: https://cyberbotics.com/doc/reference/robot?tab=python#getlightsensor
#Motors
leftMotor = robot.getMotor('motor.left')
rightMotor = robot.getMotor('motor.right')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0)
rightMotor.setVelocity(0)
#see functions of DistanceSensor: https://cyberbotics.com/doc/reference/distancesensor#wb_distance_sensor_enable
#sensors
sens2 = robot.getDistanceSensor("prox.horizontal.2")
sens4 = robot.getDistanceSensor("prox.horizontal.4")
#enable distance sensor measurements in sampling period of TIME_STEP
sens2.enable(TIME_STEP)
sens4.enable(TIME_STEP)
class ThymioRobot:
def __init__(self, id, gender, startPos, sensor_front, sensor_right):
self.id = id
self.gender = gender
self.startPos = startPos
self.sensor_front = sensor_front
self.sensor_right = sensor_right
# Choose random gender
##############
class SuperRoomba:
def __init__(self):
print('Initializing robot...')
# Create the Robot instance.
self.robot = Robot()
# distance sensors
self.ps = []
ps_names = [
'ps0', 'ps1', 'ps2', 'ps3',
'ps4', 'ps5', 'ps6', 'ps7'
]
for i in range(8):
self.ps.append(self.robot.getDistanceSensor(ps_names[i]))
self.ps[i].enable(TIME_STEP)
self.leds = []
led_names = [
'led0', 'led1', 'led2', 'led3', 'led4',
'led5', 'led6', 'led7', 'led8', 'led9'
]
for i in range(10):
self.leds.append(self.robot.getLED(led_names[i]))
self.compass = self.robot.getCompass('compass')
self.compass.enable(TIME_STEP)
self.leftMotor = self.robot.getMotor('left wheel motor')
self.rightMotor = self.robot.getMotor('right wheel motor')
self.left_encoder = self.robot.getPositionSensor('left wheel sensor')
self.right_encoder = self.robot.getPositionSensor('right wheel sensor')
self.left_encoder.enable(TIME_STEP)
self.right_encoder.enable(TIME_STEP)
self.leftMotor.setPosition(float('inf'))
self.rightMotor.setPosition(float('inf'))
self.leftMotor.setVelocity(0.0)
self.rightMotor.setVelocity(0.0)
self.touch_sensor = self.robot.getTouchSensor('touch sensor')
self.touch_sensor.enable(TIME_STEP)
self.left_vel = None
self.right_vel = None
# Current absolute direction
self.facing = None
self.facing_angle = None
self.turning = None
# start values for encoder tracking
self.left_pos_start = None
self.right_pos_start = None
# Led for search animation
self.search_led = 0
self.led_timer = 0
self.initialize_sensors()
# Current movement state
self.state = None
self.best_path = None
self.path_logger = PathLogger()
if use_file and run == 3:
self.path_logger.load(last_best_path)
self.best_path = iter(self.path_logger)
# Load initial samples in each sensor window
self.ps_windows = []
for i in range(0, 8):
self.ps_windows.append(list(repeat(self.ps[i].getValue(), WINDOW_SIZE)))
def initialize_sensors(self):
# Initialize sensors
for i in range(3):
if self.robot.step(TIME_STEP) == -1:
break
for x in range(8):
self.ps[x].getValue()
self.compass.getValues()
self.left_encoder.getValue()
self.right_encoder.getValue()
self.touch_sensor.getValue()
def write_motors(self):
self.leftMotor.setVelocity(self.left_vel)
self.rightMotor.setVelocity(self.right_vel)
def stop_motors(self):
self.left_vel = 0
self.right_vel = 0
self.write_motors()
def full_forward(self):
self.left_vel = FORWARD_SPEED
self.right_vel = FORWARD_SPEED
self.write_motors()
def full_reverse(self):
self.left_vel = -FORWARD_SPEED
self.right_vel = -FORWARD_SPEED
self.write_motors()
@staticmethod
def rot_dist(current, target):
dist = abs(target - current)
return min(dist, TWO_PI - dist)
def in_range(self, value, target, variance, angle=False):
if angle:
return self.rot_dist(value, target) <= variance
return target - variance <= value <= target + variance
@staticmethod
def get_turn_direction(current, target):
target += math.pi
current += math.pi
right = (target - current) % TWO_PI
left = (current - target) % TWO_PI
if right < left:
return Direction.RIGHT
return Direction.LEFT
def ps_refresh(self):
for i in range(0, WINDOW_SIZE):
if self.robot.step(TIME_STEP) == -1:
break
for x in range(8):
window = self.ps_windows[x]
window.insert(0, self.ps[x].getValue())
window.pop()
def get_compass(self):
x, y, z = self.compass.getValues()
return math.atan2(x, z)
def capture_position(self):
self.left_pos_start = self.left_encoder.getValue()
self.right_pos_start = self.right_encoder.getValue()
def move_distance_blocking(self, dist):
self.capture_position()
while self.robot.step(TIME_STEP) != -1:
left_pos = self.left_encoder.getValue()
right_pos = self.right_encoder.getValue()
# print('Moving -> {}%'.format(int(abs(right_pos - self.right_pos_start) / dist * 100)))
right_done = abs(right_pos - self.right_pos_start) > dist
left_done = abs(left_pos - self.left_pos_start) > dist
if right_done:
self.right_vel = 0
if left_done:
self.left_vel = 0
self.write_motors()
if right_done and left_done:
return True
return False
def turn_towards_blocking(self, target):
if self.get_turn_direction(self.get_compass(), target) == Direction.RIGHT:
self.left_vel = HALF_SPEED
self.right_vel = -HALF_SPEED
self.turning = Direction.RIGHT
else:
self.left_vel = -HALF_SPEED
self.right_vel = HALF_SPEED
self.turning = Direction.LEFT
self.write_motors()
start = self.get_compass()
total_dist = self.rot_dist(start, target)
while self.robot.step(TIME_STEP) != -1:
angle = self.get_compass()
# print('Turning -> {}%'.format(int(self.rot_dist(angle, target) / total_dist * 100)))
if self.in_range(angle, target, 0.15, angle=True):
self.left_vel = CREEP_SPEED if self.turning == Direction.RIGHT else -CREEP_SPEED
self.right_vel = -CREEP_SPEED if self.turning == Direction.RIGHT else CREEP_SPEED
self.write_motors()
if self.in_range(angle, target, 0.02, angle=True):
return True
return False
def intersection_align(self):
self.full_forward()
print('Aligning with intersection...')
self.move_distance_blocking(DIST_ALIGN)
print('Aligned.')
self.ps_refresh()
def enter_passage(self):
self.full_forward()
print('Entering passage...')
self.move_distance_blocking(DIST_ENTER)
print('Entered.')
self.ps_refresh()
def turn_around(self):
print('Turning around')
self.blink_reverse(LED_BLINK_TIME)
# angle_mid = dir_angles[Direction((self.facing.value - 1) % 4)]
# self.turn_towards_blocking(angle_mid)
# self.full_forward()
# self.move_distance_blocking(DIST_WALL_ALIGN)
self.facing = Direction((self.facing.value - 2) % 4)
self.turn_towards_blocking(dir_angles[self.facing])
def async_delay(self, delay):
time_start = time.time()
while self.robot.step(TIME_STEP) != -1:
if (time.time() - time_start) > delay:
break
def set_led_range(self, start, stop):
for i in range(start, stop):
self.leds[i].set(1)
def clear_led_range(self, start, stop):
for i in range(start, stop):
self.leds[i].set(0)
def blink_right(self, delay):
self.set_led_range(1, 4)
self.async_delay(delay)
self.clear_led_range(1, 4)
def blink_left(self, delay):
self.set_led_range(5, 8)
self.async_delay(delay)
self.clear_led_range(5, 8)
def blink_forward(self, delay):
self.set_led_range(0, 2)
self.leds[7].set(1)
self.async_delay(delay)
self.clear_led_range(0, 2)
self.leds[7].set(0)
def blink_reverse(self, delay):
self.set_led_range(3, 6)
self.async_delay(delay)
self.clear_led_range(3, 6)
def turn_right(self):
print('Turning right')
self.blink_right(LED_BLINK_TIME)
self.facing = Direction((self.facing.value + 1) % 4)
self.turn_towards_blocking(dir_angles[self.facing])
self.enter_passage()
def turn_left(self):
print('Turning left')
self.blink_left(LED_BLINK_TIME)
self.facing = Direction((self.facing.value - 1) % 4)
self.turn_towards_blocking(dir_angles[self.facing])
self.enter_passage()
def setup(self):
# Turn for first decision
self.facing = Direction.NORTH
self.turn_towards_blocking(dir_angles[self.facing])
self.led_timer = time.time()
self.state = State.DECIDE
def mainloop(self):
while self.robot.step(TIME_STEP) != -1:
# Window all the proximity sensors for moving averages
ps_values = []
for i in range(8):
window = self.ps_windows[i]
window.insert(0, self.ps[i].getValue())
window.pop()
ps_values.append(sum(window) / WINDOW_SIZE)
# Pull front sensors un-windowed so no delay
ps_front_left = self.ps[PS_F_L].getValue()
ps_front_right = self.ps[PS_F_R].getValue()
angle = self.get_compass()
if self.state == State.DECIDE:
self.stop_motors()
# print(ps_values)
# if (use_file and ps_values[PS_R] < PS_MIN) or (not use_file and ps_values[PS_L] < PS_MIN):
# # self.intersection_align()
# self.intersection_align() # twice
# self.intersection_align() # twice
#
# if (use_file and ps_values[PS_L] < PS_MIN) or (not use_file and ps_values[PS_R] < PS_MIN):
# self.intersection_align() # twice
if use_file:
if run == 3:
next_dir = next(self.best_path)
if next_dir == self.facing:
print('Proceeding {}'.format(next_dir.name.title()))
self.enter_passage()
else:
print('Turning {}'.format(next_dir.name.title()))
self.facing = next_dir
self.turn_towards_blocking(dir_angles[self.facing])
self.enter_passage()
else:
# Give priority to left
if ps_values[PS_L] < PS_MIN:
self.turn_left()
else:
if ps_front_left < PS_FRONT_STOP and ps_front_right < PS_FRONT_STOP:
print('Going forward')
self.blink_forward(LED_BLINK_TIME)
self.enter_passage()
else:
if ps_values[PS_R] < PS_MIN:
self.turn_right()
else:
self.turn_around()
if run == 2:
self.path_logger.add(self.facing)
else:
# Give priority to right
if ps_values[PS_R] < PS_MIN:
self.turn_right()
else:
# print('Not turning right because: {}'.format(ps_values[PS_R]))
# self.ps_refresh()
# print('After refreshing its {}'.format(ps_values[PS_R]))
if ps_front_left < PS_FRONT_STOP and ps_front_right < PS_FRONT_STOP:
print('Going forward')
self.blink_forward(LED_BLINK_TIME)
self.enter_passage()
else:
if ps_values[PS_L] < PS_MIN:
self.turn_left()
else:
self.turn_around()
if run == 2:
self.path_logger.add(self.facing)
self.leds[LED_BODY].set(0)
self.full_forward()
self.state = State.SEARCH
elif self.state == State.SEARCH:
# print('L: {} R: {}'.format(self.ps[PS_F_L].getValue(), self.ps[PS_F_R].getValue()))
if (time.time() - self.led_timer) > 0.1:
self.led_timer = time.time()
self.leds[self.search_led].set(0)
self.search_led = (self.search_led + 1) % 8
self.leds[self.search_led].set(1)
if self.get_turn_direction(angle, dir_angles[self.facing]) == Direction.LEFT:
self.right_vel += 0.025
self.left_vel -= 0.025
else:
self.left_vel += 0.025
self.right_vel -= 0.025
self.write_motors()
if self.touch_sensor.getValue():
self.leds[self.search_led].set(0)
self.state = State.FINISH
front_hit = self.ps[PS_F_L].getValue() > PS_FRONT_STOP or self.ps[PS_F_R].getValue() > PS_FRONT_STOP
if front_hit:
print('Encountered wall')
self.leds[LED_BODY].set(1)
self.stop_motors()
self.leds[self.search_led].set(0)
self.state = State.DECIDE
continue
if use_file:
turn_found = ps_values[PS_L] < PS_MIN
else:
turn_found = ps_values[PS_R] < PS_MIN
# either_turn_found = ps_values[PS_L] < PS_MIN or ps_values[PS_R] < PS_MIN
# both_turns_found = ps_values[PS_L] < PS_MIN and ps_values[PS_R] < PS_MIN
# if use_file:
# soft_intersection = ps_values[PS_R] < PS_MIN
# else:
# soft_intersection = ps_values[PS_L] < PS_MIN
if turn_found:
print('Encountered turn')
self.leds[LED_BODY].set(1)
self.intersection_align()
self.leds[self.search_led].set(0)
self.state = State.DECIDE
# if soft_intersection:
# self.path_logger.add(self.facing)
elif self.state == State.FINISH:
print('Finished.')
print('Path log: {}'.format(self.path_logger.chars()))
self.stop_motors()
break
def cleanup(self):
with open('data.json', 'w') as f:
data = {
'run': run + 1,
'best_path': self.path_logger.chars()
}
json.dump(data, f)
def run(self):
self.setup()
self.mainloop()
self.cleanup()
"shoulder_lift_joint_sensor")
shoulder_pan_joint_sensor = robot.getPositionSensor(
"shoulder_pan_joint_sensor")
wrist_1_joint_sensor = robot.getPositionSensor("wrist_1_joint_sensor")
wrist_2_joint_sensor = robot.getPositionSensor("wrist_2_joint_sensor")
wrist_3_joint_sensor = robot.getPositionSensor("wrist_3_joint_sensor")
#enabling sensors (each sensor needs to be enabled and you need to specify an update delay)
elbow_joint_sensor.enable(TIME_STEP)
shoulder_lift_joint_sensor.enable(TIME_STEP)
shoulder_pan_joint_sensor.enable(TIME_STEP)
wrist_1_joint_sensor.enable(TIME_STEP)
wrist_2_joint_sensor.enable(TIME_STEP)
wrist_3_joint_sensor.enable(TIME_STEP)
dist_sensor = robot.getDistanceSensor("distance sensor")
dist_sensor.enable(TIME_STEP)
# cam_sensor = robot.getCamera("camera")
# cam_sensor.enable(TIME_STEP)
# img = cam_sensor.saveImage('test_img.png',50)
# print(img)
k = 0
while robot.step(TIME_STEP) != -1:
# dist_sensor_val = dist_sensor.getValue()
# print(dist_sensor_val)
print("The elbow_joint_sensor reads: %.2f radian" %
elbow_joint_sensor.getValue())
def __init__(self, timestep: int, bot: Robot):
# self.light_sensors_positions = [1.27, 0.77, 0.0, 5.21, 4.21, 3.14159, 2.37, 1.87, 1.58784]
# self.distance_sensors_positions = [1.27, 0.77, 0.0, 5.21, 4.21, 3.14159, 2.37, 1.87]
# self.wheel_actuators_positions = [0.0, 3.14159]
# self.bump_sensors_positions = [1.27, 0.77, 0.0, 5.21, 4.21, 3.14159, 2.37, 1.87]
self.__devices_labels = EPuckDevicesLabels(
self.N_DISTANCE_SENSORS,
self.N_LIGHT_SENSORS,
self.N_BUMPERS,
self.N_LEDS,
self.N_WHEELS
)
#Retrieve device references
self.__led_actuators = actuators_array(
self.__devices_labels.led_actuators_labels,
timestep,
lambda name: bot.getLED(name)
)
self.__wheel_actuators = actuators_array(
self.__devices_labels.wheel_actuators_labels,
timestep,
lambda name: bot.getMotor(name),
actuators_positions=self.WHEEL_ACTUATORS_POSITION
)
self.__distance_sensors = sensor_array(
self.__devices_labels.distance_sensors_labels,
timestep,
lambda name: bot.getDistanceSensor(name),
sensors_positions=self.DISTANCE_SENSORS_POSITION
)
self.__light_sensors = sensor_array(
self.__devices_labels.light_sensors_labels,
timestep,
lambda name: bot.getLightSensor(name),
sensors_positions=self.LIGHT_SENSORS_POSITION
)
# self.__bumpers = sensor_array(
# self.__devices_labels.bump_sensors_labels,
# timestep,
# lambda name: bot.getTouchSensor(name),
# sensors_positions = self.bump_sensors_positions
# )
self.__gps = sensor_array(
self.__devices_labels.gps_sensors_labels,
timestep,
lambda name: bot.getGPS(name),
sensors_positions=[0.0]
)
self.__emitters = actuators_array(
self.__devices_labels.emitter_labels,
timestep,
lambda name: bot.getEmitter(name),
actuators_positions=[0.0]
)
self.__receivers = sensor_array(
self.__devices_labels.receiver_labels,
timestep,
lambda name: bot.getReceiver(name),
sensors_positions=[0.0]
)
robot = Robot()
# Get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# Get pointer to the robot wheels motors.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')
# We will use the velocity parameter of the wheels, so we need to
# set the target position to infinity.
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))
# Get and enable the distance sensors.
frontSensor = robot.getDistanceSensor("so3")
frontSensor.enable(timestep)
frontleftSensor = robot.getDistanceSensor("so1")
frontleftSensor.enable(timestep)
sideSensor = robot.getDistanceSensor("so0")
sideSensor.enable(timestep)
backsideSensor = robot.getDistanceSensor("so15")
backsideSensor.enable(timestep)
maxdist = 0.5
# Move forward until we are 50 cm away from the wall.
leftWheel.setVelocity(MAX_SPEED)
rightWheel.setVelocity(MAX_SPEED)
while robot.step(timestep) != -1:
if getDistance(frontSensor) < 0.25:
break
MAX_SPEED = 6.28 # максимальная скорость
robot = Robot()
timestep = int(robot.getBasicTimeStep())
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)
rightFor = robot.getDistanceSensor('ps0') # подключаем датчики расстояния:
rightFor.enable(timestep) # правый и передний правый,
right = robot.getDistanceSensor('ps2') # а также активируем их
right.enable(timestep)
# счётчики для выполнения определённых действий
cntForward = 0 # для движения вперёд после потери препятствия
cntLeft = 0 # для поворота налево
cntRight = 0 # для поворота направо
cntForwardAfterRotation = 0 # для движения вперёд после поворота
# коэффициенты скорости моторов
leftSpeed = 0.0
rightSpeed = 0.0
while robot.step(timestep) != -1:
rf = rightFor.getValue() # получение данных
import math
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
#enable distance sensors
frontDistanceSensor = robot.getDistanceSensor('front_ds')
leftDistanceSensor = robot.getDistanceSensor('left_ds')
rightDistanceSensor = robot.getDistanceSensor('right_ds')
frontDistanceSensor.enable(timestep)
leftDistanceSensor.enable(timestep)
rightDistanceSensor.enable(timestep)
# enable camera and recognition
camera = robot.getCamera('camera1')
camera.enable(timestep)
camera.recognitionEnable(timestep)
#enable imu
imu = robot.getInertialUnit('inertial unit')
imu.enable(timestep)
emitter = robot.getEmitter("emitter")
gps = robot.getGPS("gps")
gps.enable(timeStep)
left_heat_sensor = robot.getLightSensor("left_heat_sensor")
right_heat_sensor = robot.getLightSensor("right_heat_sensor")
left_heat_sensor.enable(timeStep)
right_heat_sensor.enable(timeStep)
leftSensors = []
rightSensors = []
frontSensors = []
frontSensors.append(robot.getDistanceSensor("ps7"))
frontSensors[0].enable(timeStep)
frontSensors.append(robot.getDistanceSensor("ps0"))
frontSensors[1].enable(timeStep)
rightSensors.append(robot.getDistanceSensor("ps1"))
rightSensors[0].enable(timeStep)
rightSensors.append(robot.getDistanceSensor("ps2"))
rightSensors[1].enable(timeStep)
leftSensors.append(robot.getDistanceSensor("ps5"))
leftSensors[0].enable(timeStep)
leftSensors.append(robot.getDistanceSensor("ps6"))
leftSensors[1].enable(timeStep)
# [left wheel speed, right wheel speed]
motors = []
encoders = []
for i, motor_name in enumerate(["left", "right"]):
motor = robot.getMotor('{} wheel motor'.format(motor_name))
enc = robot.getPositionSensor('{} wheel encoder'.format(motor_name))
motor.setPosition(float('inf'))
motor.setVelocity(0.0)
enc.enable(timestep)
motors.append(motor)
encoders.append(enc)
# distance sensors:
distance_sensors = []
for i in range(2):
sensor = robot.getDistanceSensor("ds{:}".format(i))
sensor.enable(timestep)
distance_sensors.append(sensor)
camera = robot.getCamera("camera")
camera.enable(timestep)
max_vel = motors[1].getMaxVelocity()
print("Max velocity is {:.3f} rad/s".format(max_vel))
speed_vectors = [1, -1]
ds_to_vel = 0.004
speed_offset = 0.3 * max_vel
# Main loop:
wheels = []
wheelsNames = ['wheel1', 'wheel2']
for i in range(2):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
robot_type = 1
communication = robot.getReceiver("Receiver")
Receiver.enable(communication, TIME_STEP)
ds0 = robot.getDistanceSensor("ds0")
ds1 = robot.getDistanceSensor("ds1")
ds0.enable(TIME_STEP)
ds1.enable(TIME_STEP)
# Main loop:
while robot.step(TIME_STEP) != -1:
# Just receiver stuff...
if (Receiver.getQueueLength(communication) > 0) :
# /* read current packet's data */
new_message= receiver.getData()
message = struct.unpack("ddd", new_mssage)
if (message_printed != 1) :
"right ultrasonic sensor"
]
IRSensorNames = [
"rear left infrared sensor", "left infrared sensor",
"front left infrared sensor", "front infrared sensor",
"front right infrared sensor", "right infrared sensor",
"rear right infrared sensor", "rear infrared sensor",
"ground left infrared sensor", "ground front left infrared sensor",
"ground front right infrared sensor", "ground right infrared sensor"
]
USSensors = []
IRSensors = []
#добавляем все ультразвуковые сенсоры в список
for sensor in USSensorNames:
USSensors.append(robot.getDistanceSensor(sensor))
#добавляем все ИК сенсоры в список
for sensor in IRSensorNames:
IRSensors.append(robot.getDistanceSensor(sensor))
#включаем все сенсоры
for sensor in USSensors:
sensor.enable(TIME_STEP)
for sensor in IRSensors:
sensor.enable(TIME_STEP)
#включаем моторы, устанавливаем начальную скорость = 0
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
leftMotor.setPosition(float('inf'))
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from controller import Robot
robot = Robot()
timestep = int(robot.getBasicTimeStep())
print('Move forward until an obstabcle is detected.')
robot.getLED('motor led').set(0xFF0000)
robot.getLED('distance sensor led').set(0x00FF00)
motor = robot.getMotor('motor')
motor.setPosition(float('inf')) # Velocity control mode.
motor.setVelocity(0.5 * motor.getMaxVelocity())
distanceSensor = robot.getDistanceSensor('distance sensor')
distanceSensor.enable(timestep)
while robot.step(timestep) != -1:
distance = distanceSensor.getValue()
if distance < 100:
print('Obstacle detected. Stop the motor.')
motor.setVelocity(0)
robot.step(timestep)
break
from controller import Robot
SPEED = 1.0
robot = Robot()
timestep = int(robot.getBasicTimeStep())
left_motor = robot.getMotor('left wheel motor')
right_motor = robot.getMotor('right wheel motor')
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)
ds_c = robot.getDistanceSensor('floor sensor')
ds_c.enable(timestep)
iter = 0
flag = True
while robot.step(timestep) != -1:
iter += 1
value = ds_c.getValue()
if value < 101:
left_motor.setVelocity(-SPEED)
right_motor.setVelocity(SPEED)
continue
if iter % 10 == 0:
if flag:
SPEED += 0.1
else:
SPEED -= 0.2
if SPEED == 10 or SPEED == 2:
flag = not flag
"""e-puck_go_forward controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot
robot = Robot()
MAX_SPEED = 5.24
DELAY = 70
timestep = int(robot.getBasicTimeStep())
left_wheel = robot.getMotor('left wheel')
right_wheel = robot.getMotor('right wheel')
front_ss = robot.getDistanceSensor('so4')
front_ss.enable(timestep)
leftSensors = []
for i in range(3):
sensor = robot.getDistanceSensor('so' + str(i))
leftSensors.append(sensor)
leftSensors[i].enable(timestep)
rightSensors = []
for i in range(3):
sensor = robot.getDistanceSensor('so' + str(5 + i))
rightSensors.append(sensor)
rightSensors[i].enable(timestep)
# Get reference to the robot.
robot = Robot()
# Get simulation step length.
timeStep = int(robot.getBasicTimeStep())
# Constants of the Thymio II motors and distance sensors.
maxMotorVelocity = 9.53
distanceSensorCalibrationConstant = 360
# Get left and right wheel motors.
leftMotor = robot.getMotor("motor.left")
rightMotor = robot.getMotor("motor.right")
# Get frontal distance sensors.
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 sensors.
outerLeftSensor.enable(timeStep)
centralLeftSensor.enable(timeStep)
centralSensor.enable(timeStep)
centralRightSensor.enable(timeStep)
outerRightSensor.enable(timeStep)
# Disable motor PID control mode.
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
