frontDistanceSensor = robot.getDistanceSensor('front_ds')
leftDistanceSensor = robot.getDistanceSensor('left_ds')
rightDistanceSensor = robot.getDistanceSensor('right_ds')
frontDistanceSensor.enable(timestep)
leftDistanceSensor.enable(timestep)
rightDistanceSensor.enable(timestep)
# get left and right light sensors
leftLightSensor = robot.getLightSensor('lls')
rightLightSensor = robot.getLightSensor('rls')
leftLightSensor.enable(timestep)
rightLightSensor.enable(timestep)
# get left and right LED
leftLED = robot.getLED('left_led')
rightLED = robot.getLED('right_led')
leftLED.set(1)
rightLED.set(1)
# enable camera and recognition
camera = robot.getCamera('camera')
camera.enable(timestep)
camera.recognitionEnable(timestep)
# get handler to motors and set target position to infinity
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0)
def randomizeAmplitude():
return 0.4 + (random.random() / 2.5 - 0.2)
# create the Supervisor instance.
supervisor = Supervisor()
speaker = supervisor.getSpeaker("fire_speaker")
Speaker.playSound(speaker, speaker, "fire_burning.wav", 0.1, 1, 0, True)
# get the time step of the current world.
timestep = int(supervisor.getBasicTimeStep())
led = supervisor.getLED('EmberLED')
lastLedIntensity = 0
currentStep = 0
random.seed()
ledBaseLevel = 0.6
amplitude = randomizeAmplitude()
period = randomizePeriod()
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while supervisor.step(timestep) != -1:
ledIntensity = led.get()
ledIntensity = (ledBaseLevel + math.sin(currentStep * period) * amplitude)
pI = 3.14159265
curr_angle = 0
#initialize GPS
gps = supervisor.getGPS('dummy_gps')
gps.enable(TIME_STEP)
prev_data = [0, 0, 0]
# for getting average distance change
n = 0
avg = 0
cumulative = 0
#LED stuff
ledR = [None] * 5
ledR[0] = supervisor.getLED('fire_1')
ledR[1] = supervisor.getLED('fire_2')
ledR[2] = supervisor.getLED('fire_3')
ledR[3] = supervisor.getLED('fire_4')
ledR[4] = supervisor.getLED('fire_5')
led_on = 0
led_num = random.randint(0, 4)
def isStuck(new_data, prev_data):
xd = (new_data[0] - prev_data[0])
yd = (new_data[1] - prev_data[1])
zd = (new_data[2] - prev_data[2])
if xd < STUCK_D and yd < STUCK_D and zd < STUCK_D:
return 1
else:
