soln = np.zeros((len(time),len(y)))
# Create instance of PID_Controller class and
# initalize and set all the variables
pid = PIDController(kp = kp, ki = ki, kd = kd, max_windup = 1e6, u_bounds
= [0, umax], alpha = alpha)
# Set altitude target
r = 10 # meters
pid.setTarget(r)
# Simulate quadrotor motion
j = 0 # dummy counter
for t in time:
# Evaluate state at next time point
y = ydot(y,t,pid)
# Store results
soln[j,:] = y
j += 1
##################################################################################
# Plot results
SP = np.ones_like(time)*r # altitude set point
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:,0],time,SP,'--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:,1])
# Create instance of Open_Controller class
controller = Open_Controller()
# Set our contstant control effort
controller.setControlEffort(control_effort)
# Set altitude target
r = 10 # meters
controller.setTarget(r)
# Simulate quadrotor motion
j = 0 # dummy counter
for t in time:
# Evaluate state at next time point
y = ydot(y, t, controller)
# Store results
soln[j, :] = y
j += 1
##################################################################################
# Plot results
# Plot 1: This is the altitude of our quad copter as a function of time!
SP = np.ones_like(time) * r # altitude set point
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
# Plot 2: This is the speed of our quad copter as a function of time!
# Set the Kp value of the controller
pi.setKP(kp)
# Set the Ki value of the controller
pi.setKI(ki)
# Set altitude target
r = 10 # meters
pi.setTarget(r)
# Simulate quadrotor motion
j = 0 # dummy counter
for t in time:
# Evaluate state at next time point
y = ydot(y, t, pi)
# Store results
soln[j, :] = y
j += 1
##################################################################################
# Plot results
SP = np.ones_like(time) * r # altitude set point
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
# Create instance of P_Controller class
p = P_Controller()
# Set the Kp value of the controller
p.setKP(kp)
# Set altitude target
r = 10 # meters
p.setTarget(r)
# Simulate quadrotor motion
j = 0 # dummy counter
for t in time:
# Evaluate state at next time point
y = ydot(y, t, p)
# Store results
soln[j, :] = y
j += 1
##################################################################################
# Plot results
SP = np.ones_like(time) * r # altitude set point
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:, 0], time, SP, '--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:, 1])
# Set the Kp value of the controller
pd.setKP(kp)
# Set the Kd value of the controller
pd.setKD(kd)
# Set altitude target
r = 10 # meters
pd.setTarget(r)
# Simulate quadrotor motion
j = 0 # dummy counter
for t in time:
# Evaluate state at next time point
y = ydot(y,t,pd)
# Store results
soln[j,:] = y
j += 1
##################################################################################
# Plot results
SP = np.ones_like(time)*r # altitude set point
fig = plt.figure()
ax1 = fig.add_subplot(211)
ax1.plot(time, soln[:,0],time,SP,'--')
ax1.set_xlabel('Time, (sec)')
ax1.set_ylabel('Altitude, (m)')
ax2 = fig.add_subplot(212)
ax2.plot(time, soln[:,1])
