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]))
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
yaw_giris=yaw_dagitim
keyboard = Keyboard()
keyboard.enable(60)
while robot.step(timestep) != -1:
#motor_lst[0].setPosition(10.0)
#motor_lst[0].setVelocity(1.0)
key=keyboard.getKey()
if (key==ord('D')):
print('D is pressed')
rotate(math.pi * -2 / 8)
elif (key==ord('F')):
print('F is pressed')
rotate(math.pi * 2 / 8)
elif (key==ord(' ')):
print('Space is pressed')
default_high_pos()
elif (key==ord('V')):
print('V is pressed')
default_low_pos()
print("gyro", gyro.getValues())
print("inertial_unit", inertial_unit.getRollPitchYaw())
#rotate()
#move_forward()
# Enter here exit cleanup code.
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