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]))
def loc():
return GPS.getValues(gp)
# 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.
def loc():
return GPS.getValues(gp)
key = keyboard.getKey() #allow to get keyboard command
if key == ord('D') or key == ord('d'):
#set leftwheel speed as 10.0 rad/s to turn right
buttonLeftSpeed = 10.0
buttonRightSpeed = 0.0
elif key == ord('A') or key == ord('a'):
#set rightwheel speed as 10.0 rad/s to turn left
buttonLeftSpeed = 0.0
buttonRightSpeed = 10.0
else:
#if there is no keyboard command, vehicle will stop
buttonLeftSpeed = 0.0
buttonRightSpeed = 0.0
time += TIME_STEP
read_position = gps.getValues() # GPS returns a 3D-vector
error = target_position - read_position[0]
#while calculating the error, x-coordinate is used.
gps_log.append(read_position[0])
controlled_velocity = Controller(Kp, error, TIME_STEP)
if error > 0:
# error > 0 means the vehicle is in the left side of the road
leftSpeed = carSpeed - controlled_velocity
rightSpeed = carSpeed + controlled_velocity
if time > 300000:
# After 300 seconds the vehicle will stop
leftSpeed = 0.0
rightSpeed = 0.0
elif error < 0:
# error < 0 means the vehicle is in the right side of the road
while robot.step(timestep) != -1:
if(robot.getTime()>1.0):
break
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while robot.step(timestep) != 1:
roll=imu.getRollPitchYaw()[0]+M_PI/2.0
pitch=imu.getRollPitchYaw()[1]
rakim=gps.getValues()[1]
roll_hiz=gyro.getValues()[0]
pitch_hiz=gyro.getValues()[1]
print("x ekseni : {0} - y ekseni : {1} ".format(roll,pitch))
camera_roll_motor.setPosition(0.1*roll_hiz)
camera_pitch_motor.setPosition(0.1*pitch_hiz)
roll_dagitim=0.0
pitch_dagitim=0.0
yaw_dagitim=0.0
target_rakim=1.0
roll_giris=k_roll_p*CLAMP(roll,-1.0,1.0)+roll_hiz+roll_dagitim
pitch_giris=k_pitch_p*CLAMP(pitch,-1.0,1.0)-pitch_hiz+pitch_dagitim
Kd = 0.5 #Derivative Gain
gps_log = []
error_log = []
previous_error = 0
error = 0
def Controller(Kp,Ki,Kd,Error,cumulative_error,previous_error,TIME_STEP): #PID-controller function
controlled_velocity = (Error * Kp + cumulative_error * Ki + previous_error * Kd) / (2*math.pi)
#controlled velocity which is converted to rad/s
return controlled_velocity
while robot.step(TIME_STEP) != -1: # Main loop:
# - perform simulation steps until Webots is stopping the controller
time +=TIME_STEP
read_position= gps.getValues()
previous_error = error
error = target_position-read_position[0]
error_log.append(error)
cumulative_error = np.sum(error_log)*(TIME_STEP*1e-3)
gps_log.append(read_position[0])
controlled_velocity = Controller(Kp,Ki,Kd,error,cumulative_error,previous_error,TIME_STEP)
if error > 0:
# error > 0 means the vehicle is in the left side of the road
leftSpeed = carSpeed-controlled_velocity
rightSpeed = carSpeed+controlled_velocity
if time>300000:
# After 300 seconds the vehicle will stop
leftSpeed = 0.0
rightSpeed = 0.0
# Declare Target location from Supervisor call
target_x = goal_location[0]
target_y = goal_location[2]
state = "find target"
#state = "landing"
###
# Main loop:
###
# - perform simulation steps until Webots stops the controller
while robot.step(timestep) != -1:
print(" ") # For clarity between timesteps
# GPS
gps_x, altitude, gps_y = gps.getValues()
# for altitude acceleration
target_altitude = 1
err_x = gps_x - target_x
err_y = gps_y - target_y
# for positional acceleration in terms of target
if last_x is None:
velocity_x = 0
else:
velocity_x = (gps_x - last_x) / timestep
if last_y is None:
velocity_y = 0
else:
velocity_y = (gps_y - last_y) / timestep
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