def run(self):
while True:
# Read the supervisor order.
if self.receiver.getQueueLength() > 0:
message = self.receiver.getData().decode('utf-8')
self.receiver.nextPacket()
print('I should ' + message + '!')
#if message == 'avoid obstacles':
#self.mode = self.Mode.AVOIDOBSTACLES
#elif message == 'move forward':
# self.mode = self.Mode.MOVE_FORWARD
# elif message == 'stop':
#self.mode = self.Mode.STOP
#elif message == 'turn':
#self.mode = self.Mode.TURN
delta = self.distanceSensors[0].getValue() - self.distanceSensors[1].getValue()
speeds = [0.0, 0.0]
gps = GPS("ambu")
gps.enable(self.timeStep)
gpsvalues = []
gpsvalues.extend(gps.getValues())
gps1stvalue = gpsvalues[0]
gps11stvalue = round(gps1stvalue, 2)
gps2ndvalue = gpsvalues[1]
gps2ndvalue = round(gps2ndvalue, 2)
gps3rdvalue = gpsvalues[2]
gps3rdvalue =round(gps3rdvalue, 2)
new_gst_values = np.arange(0.5,0.9,0.01)
#position = []
#position = gps.getValues()
#latitude = GPS.convertToDegreesMinutesSeconds(position[0])
#altitude = position[2]
#longitude = GPS.convertToDegreesMinutesSeconds(position[1])
#total_speed = gps.getSpeed(gps)
#print(latitude)
#print(gpsvalues)
#lightsensor = LightSensor("light")
#lightsensor = self.getLightSensor ('light')
#ligfhtsensor.enable(self.timestep)
# Send actuators commands according to the mode.
if self.mode == self.Mode.AVOIDOBSTACLES:
speeds[0] = self.boundSpeed(self.maxSpeed / 2 + 0.1 * delta)
speeds[1] = self.boundSpeed(self.maxSpeed / 2 - 0.1 * delta)
elif self.mode == self.Mode.MOVE_FORWARD:
speeds[0] = self.maxSpeed
speeds[1] = self.maxSpeed
elif self.mode == self.Mode.TURN:
speeds[0] = self.maxSpeed / 2
speeds[1] = -self.maxSpeed / 2
self.motors[0].setVelocity(speeds[0])
self.motors[1].setVelocity(speeds[1])
# Perform a simulation step, quit the loop when
# Webots is about to quit.
if self.step(self.timeStep) == -1:
break
def startSensors(self, timestep):
# Instantiate objects and start up GPS, Gyro, and Compass sensors
# For more details, refer to the Webots documentation
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.compass = Compass("compass")
self.compass.enable(timestep)
def startSensors(self, timestep):
""" Start sensors.
Instantiate objects and start up GPS, Gyro, IMU sensors.
For more details, refer to the Webots documentation.
Args:
timestep (int): time step of the current world.
"""
self.gps = GPS("gps")
self.gps.enable(timestep)
self.gyro = Gyro("gyro")
self.gyro.enable(timestep)
self.imu = InertialUnit("inertial unit")
self.imu.enable(timestep)
self.timestep = timestep
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
import sys
from controller import Robot, Motor, Keyboard
from controller import GPS
import time
import numpy as np
import matplotlib.pyplot as plt
import math
TIME_STEP = 64 #timestep should be the same as in the WorldInfo
# create the Robot, Keyboard, GPS instance.
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
gps = GPS('global')
gps.enable(TIME_STEP)
# assign the motors to wheels.
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(
float('inf')) #set position infinity not to restrict motion
wheels[i].setVelocity(5.0) #set initial speed as 5.0 rad/s
print('Click the simulation window before the keyboard command!')
print('Press [D] or [d] to give right input')
print('Press [A] or [a] to give left input')
time = 0
k_vertical_p=3.0
k_vertical_thrust=68.5
k_vertical_offset=0.6
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
camera=robot.getCamera("camera")
Camera.enable(camera,timestep)
imu=InertialUnit("inertial unit")
imu.enable(timestep)
pusula=Compass("compass")
gyro=Gyro("gyro")
pusula.enable(timestep)
gyro.enable(timestep)
gps=GPS("gps")
gps.enable(timestep)
# motorların tagını getirir
#motorları getirir
solMotorİleri=robot.getMotor("front left propeller")
sağMotorİleri=robot.getMotor("front right propeller")
sağMotorGeri=robot.getMotor("rear right propeller")
solMotorGeri=robot.getMotor("rear left propeller")
#motorları hareket etirir
solMotorİleri.setPosition(float("inf"))
solMotorGeri.setPosition(float("inf"))
sağMotorİleri.setPosition(float("inf"))
sağMotorGeri.setPosition(float("inf"))
leftMotor.setVelocity(leftSpeed)
rightMotor.setVelocity(rightSpeed)
robot.step(timestep)
updatePsValues()
if psValues[2] > closedDistance:
isUpdate = 1
closedDistance = psValues[2]
elif psValues[2] < Dwth:
continue
else:
break
# create the Robot instance.
robot = Robot()
gps = GPS(name="e-puck-gps")
# get the time step of the current world.
timestep = 64
MAX_SPEED = 6.28
Dwth = 160
w_d2 = 0.7
time90 = 12
# 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)
ps = []
psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
for i in range(8):
# position camera
camera_pitch_motor = robot.getMotor('camera pitch')
camera_pitch_motor.setPosition(math.pi / 2)
# Start propellers
initializeMotors(robot)
prop_takeoff_speed = 68.5
motorsSpeed(robot, prop_takeoff_speed, prop_takeoff_speed, prop_takeoff_speed,
prop_takeoff_speed)
# Initialize Gyro
myGyro = robot.getGyro('gyro')
myGyro.enable(timestep)
# Initialize InertialUnit
inertial_unit = InertialUnit('inertial unit')
inertial_unit.enable(timestep)
# Initialize GPS
gps = GPS('gps')
gps.enable(timestep)
#print("Coord Sys: ", gps.getCoordinateSystem())
# Initial Robot position
upright_roll = -math.pi / 2
upright_pitch = 0
upright_yaw = 0
# Initialize values for velocity calculation
last_altitude = 0
last_x = None
last_y = None
# Declare Target location from Supervisor call
target_x = goal_location[0]