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
class BaseController():
""" The base controller class.
"""
def __init__(self, robot, lossOfThrust = 0):
""" Base controller __init__ method.
Initialize drone parameters here.
Args:
robot (webots controller object): Controller for the drone.
lossOfThrust (float): percent lost of thrust.
"""
# Initialize variables
self.robot = robot
self.timestep = 0
# intializa percent loss of thrust
self.lossOfThrust = lossOfThrust
# Define robot parameter
self.m = 0.4
self.d1x = 0.1122
self.d1y = 0.1515
self.d2x = 0.11709
self.d2y = 0.128
self.Ix = 0.000913855
self.Iy = 0.00236242
self.Iz = 0.00279965
# define constants
self.g = 9.81
self.ct = 0.00026
self.ctau = 5.2e-06
self.U1_max = 10
self.pi = 3.1415926535
# define H matrix for conversion from control input U to motor speeds
self.H_inv = self.ct*np.array([[1, 1, 1, 1],
[self.d1y, -self.d1y, self.d2y, -self.d2y],
[-self.d1x, -self.d1x, self.d2x, self.d2x],
[-self.ctau/self.ct, self.ctau/self.ct, self.ctau/self.ct, -self.ctau/self.ct]
])
self.H = np.linalg.inv(self.H_inv)
# define variables for speed calculations
self.xGPS_old = 0
self.yGPS_old = 0
self.zGPS_old = 0.099019
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
def getStates(self):
""" Get drone state.
The state of drone is 16 dimensional:
xGPS, yGPS, zGPS,
roll, pitch, yaw,
x_vel, y_vel, z_vel,
roll_rate, pitch_rate, yaw_rate
Returns:
np.array: x_t. information of 12 states.
"""
# Timestep returned by Webots is in ms, so we convert
delT = 1e-3*self.timestep
# Extract (X, Y, Z) coordinate from GPS
xGPS = self.gps.getValues()[0]
yGPS = -self.gps.getValues()[2]
zGPS = self.gps.getValues()[1]
# Find the rate of change in each axis, and store the current value of (X, Y, Z)
# as previous (X, Y, Z) which will be used in the next call
x_vel = (xGPS - self.xGPS_old)/delT
y_vel = (yGPS - self.yGPS_old)/delT
z_vel = (zGPS - self.zGPS_old)/delT
self.xGPS_old = xGPS
self.yGPS_old = yGPS
self.zGPS_old = zGPS
# Extract (roll, pitch, yaw) angle from imu
roll = self.imu.getRollPitchYaw()[0]
pitch = -self.imu.getRollPitchYaw()[1]
yaw = self.imu.getRollPitchYaw()[2]
# Extract (roll rate, pitch rate, yaw rate) angular velocity from imu
roll_rate = self.gyro.getValues()[0]
pitch_rate = -self.gyro.getValues()[2]
yaw_rate = self.gyro.getValues()[1]
x_t = np.array([xGPS, yGPS, zGPS, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_rate, pitch_rate, yaw_rate]).reshape(-1,1)
return x_t
def getMotorAll(self):
""" Get each motors' controller.
Returns:
list: Each motor's controller.
"""
frontLeftMotor = self.robot.getMotor('front left propeller')
frontRightMotor = self.robot.getMotor('front right propeller')
backLeftMotor = self.robot.getMotor('rear left propeller')
backRightMotor = self.robot.getMotor('rear right propeller')
return [frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor]
def initializeMotors(self):
""" Initialisze all motors speed to 0.
"""
[frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = self.getMotorAll()
frontLeftMotor.setPosition(float('inf'))
frontRightMotor.setPosition(float('inf'))
backLeftMotor.setPosition(float('inf'))
backRightMotor.setPosition(float('inf'))
self.motorsSpeed(0, 0, 0, 0)
def motorsSpeed(self, v1, v2, v3, v4):
""" Set each motors' speed.
Args:
v1, v2, v3, v4 (int): desired speed for each motor.
"""
[frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor] = self.getMotorAll()
frontLeftMotor.setVelocity(v1)
frontRightMotor.setVelocity(v2)
backLeftMotor.setVelocity(v3)
backRightMotor.setVelocity(v4)
def convertUtoMotorSpeed(self, U):
""" Convert control input to motor speed.
Args:
U (np.array): desired control input.
Returns:
np.array: rotorspeed. Desired rotor speed.
"""
w_squre = np.clip(np.matmul(self.H, U), 0, 576**2)
rotorspeed = np.sqrt(w_squre.flatten())
return rotorspeed
def setMotorsSpeed(self, motorspeed, motor_failure=0):
""" Set motor speed.
Args:
motorspeed (np.array): desired motor speed.
motor_failure (bool): True for motor failure, False otherwise.
"""
if motor_failure :
# print("--- Motor Failure ---")
factor = np.sqrt(1 - self.lossOfThrust)
self.motorsSpeed(float(motorspeed[0]) * factor, float(-motorspeed[1]), float(-motorspeed[2]), float(motorspeed[3]))
else:
self.motorsSpeed(float(motorspeed[0]), float(-motorspeed[1]), float(-motorspeed[2]), float(motorspeed[3]))
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot, GPS
# 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)
gp = robot.getGPS("gps")
GPS.enable(gp, 64)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while robot.step(timestep) != -1:
# Read the sensors:
# Enter here functions to read sensor data, like:
# val = ds.getValue()
print(GPS.getValues(gp))
# Process sensor data here.
# Enter here functions to send actuator commands, like:
# motor.setPosition(10.0)
pass
# Enter here exit cleanup code.
# 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
leftSpeed = 0.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"))
#camera motorlarını çağıralım
class BaseController():
def __init__(self, trajectory):
# Initialize variables
self.trajectory = trajectory
self.previousX = 0
self.previousY = 0
self.previousZ = 0
self.previousPsi = 0
self.previousXdotError = 0
self.integralXdotError = 0
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 getStates(self, timestep):
# Timestep returned by Webots is in ms, so we convert
delT = 0.001 * timestep
# Extract (X, Y) coordinate from GPS
position = self.gps.getValues()
X = position[0]
Y = position[1]
# Find the rate of change in each axis, and store the current value of (X, Y)
# as previous (X, Y) which will be used in the next call
Xdot = (X - self.previousX) / (delT + 1e-9)
self.previousX = X
Ydot = (Y - self.previousY) / (delT + 1e-9)
self.previousY = Y
XYdot = np.array([[Xdot], [Ydot]])
# Get heading angle and angular velocity
psi = wrapToPi(self.getBearingInRad())
angularVelocity = self.gyro.getValues()
psidot = angularVelocity[2]
# Get the rotation matrix (2x2) to convert velocities to the vehicle frame
rotation_mat = np.array([[np.cos(psi), -np.sin(psi)],
[np.sin(psi), np.cos(psi)]])
xdot = (np.linalg.inv(rotation_mat) @ XYdot)[0, 0]
ydot = (np.linalg.inv(rotation_mat) @ XYdot)[1, 0]
# Clamp xdot above 0 so we don't have singular matrices
xdot = clamp(xdot, 1e-5, np.inf)
return delT, X, Y, xdot, ydot, psi, psidot
def getBearingInRad(self):
# Get compass relative north vector
north = self.compass.getValues()
# Calculate vehicle's heading angle from north
rad = np.arctan2(north[1], north[0])
# Convert to vehicle's heading angle from x-axis
bearing = np.pi / 2.0 - rad
return bearing