Skip to content
Snippets Groups Projects
Verified Commit 8adb0ad1 authored by Trim Bresilla's avatar Trim Bresilla
Browse files

ADD: added controller

parent c0e0394e
No related branches found
No related tags found
No related merge requests found
from controller import Robot, DistanceSensor, Motor
import numpy as np
# time in [ms] of a simulation step
TIME_STEP = 64
MAX_SPEED = 6.28
# create the Robot instance.
robot = Robot()
# initialize proximity sensors
ps = []
psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
# initialize light sensors
ls = []
lsNames = ['ls0', 'ls1', 'ls2', 'ls3', 'ls4', 'ls5', 'ls6', 'ls7']
for i in range(8):
ps.append(robot.getDevice(psNames[i]))
ls.append(robot.getDevice(lsNames[i]))
ps[i].enable(TIME_STEP)
ls[i].enable(TIME_STEP)
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)
# feedback loop: step simulation until receiving an exit event
while robot.step(TIME_STEP) != -1:
# initialize motor speeds at 50% of MAX_SPEED.
leftSpeed = 0.5 * MAX_SPEED
rightSpeed = 0.5 * MAX_SPEED
# read sensors outputs
psValues = []
lsValues = []
for i in range(8):
psValues.append(ps[i].getValue())
lsValues.append(ls[i].getValue())
# detect light
left_light = np.mean([lsValues[5], lsValues[6], lsValues[7]])
right_light = np.mean([lsValues[0], lsValues[1], lsValues[2]])
print("LEFT LIGHT: " + str(left_light))
print("RIGHT LIGHT: " + str(right_light))
# 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
# modify speeds according to obstacles
if left_obstacle:
# turn right
leftSpeed = 0.5 * MAX_SPEED
rightSpeed = -0.5 * MAX_SPEED
elif right_obstacle:
# turn left
leftSpeed = -0.5 * MAX_SPEED
rightSpeed = 0.5 * MAX_SPEED
# write actuators inputs
leftMotor.setVelocity(leftSpeed)
rightMotor.setVelocity(rightSpeed)
\ No newline at end of file
worlds/.world.jpg

7.71 KiB | W: | H:

worlds/.world.jpg

27.8 KiB | W: | H:

worlds/.world.jpg
worlds/.world.jpg
worlds/.world.jpg
worlds/.world.jpg
  • 2-up
  • Swipe
  • Onion skin
Webots Project File version R2022b
perspectives: 000000ff00000000fd00000002000000010000012b000003f7fc0200000001fb0000001400540065007800740045006400690074006f00720000000315000000f50000008900ffffff0000000300000768000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007680000006900ffffff000007680000031c00000001000000020000000100000008fc00000000
perspectives: 000000ff00000000fd0000000200000001000001a50000031cfc0200000001fb0000001400540065007800740045006400690074006f007201000000130000031c0000008900ffffff0000000300000768000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007680000006900ffffff000005c10000031c00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000014a0000061c0100000002010000000100
sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000200
maximizedDockId: -1
centralWidgetVisible: 1
orthographicViewHeight: 1
textFiles: 0 "../../../braitenberg/controllers/my_controller/my_controller.py"
textFiles: 0 "controllers/my_controller/my_controller.py"
consoles: Console:All:All
renderingDevicePerspectives: e-puck:camera;1;3.82051;0;0
......@@ -11,8 +11,8 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/project
WorldInfo {
}
Viewpoint {
orientation -0.4914730233230598 0.5930757775839278 0.6377424161751437 2.0409470469297815
position 1.4832532935708094 -1.171556534225489 9.603993544275712
orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944
position 0.2886112430982633 0.30112288717536395 4.7561144404849145
}
TexturedBackground {
}
......@@ -25,20 +25,19 @@ RectangleArena {
}
}
E-puck {
hidden position_0_0 3.5033479526508002
hidden position_0_1 3.8490546987730165
hidden linearVelocity_0 0.05577124278501405 0.006588328155072721 5.194158954614228e-06
hidden angularVelocity_0 0.0008655080265496573 -0.007385937321261792 0.08314694294448588
hidden translation_1 0 0.026000000000000006 0.02
hidden rotation_1 0 1 0 3.5033479526508002
hidden linearVelocity_1 0.05358773702595218 0.006330351253688177 5.140912414784702e-06
hidden angularVelocity_1 -0.31536512877933764 2.6691844403471228 0.08490328190137644
hidden translation_2 0 -0.025999999999999992 0.02
hidden rotation_2 0 0.9999999999999999 0 3.8490546987730165
hidden linearVelocity_2 0.05788136038354241 0.006838251846883248 5.13749240400867e-06
hidden angularVelocity_2 -0.34305197020803013 2.904160101343467 0.08517949728238716
translation 0.07155681687410703 0.27425174005292685 -6.497900169800605e-05
rotation 0.0007192736926101425 0.00419341346196072 0.9999909489234851 0.11900802352863876
hidden position_0_0 28.033302105971305
hidden position_0_1 27.431657135094095
hidden linearVelocity_0 -0.01040089258177146 0.0005189791645317463 -0.00010164286476468898
hidden angularVelocity_0 -0.012580145309195254 -0.6870843501448997 -2.2292946089286447
hidden rotation_1 0 -1 0 6.162461784455415
hidden linearVelocity_1 0.043782987222245516 -0.0016398885897364905 -2.215561220686446e-05
hidden angularVelocity_1 0.09747530080179415 2.4512349752960945 -2.23409528946547
hidden rotation_2 0 1 0 6.147970605148766
hidden linearVelocity_2 -0.07125569725759662 0.002336362298856977 -3.4872247586914343e-06
hidden angularVelocity_2 -0.12270046057049977 -3.8254049928433136 -2.2324546810944326
translation 0.6222753536792252 0.3401916142101004 -6.850241694632969e-05
rotation 0.004217783566957383 -0.029762230724924357 -0.9995481078587757 0.09378488003841622
controller "my_controller"
}
OilBarrel {
translation 0 -0.34 0.11
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment