def simulate():
t_el = 0.1
te = 25.0
if args.t: te = args.t
n = 0
h_hz = 0.1
z_hz = 0.05
hr = sigGen.squareWave(1, 2, h_hz, P.Ts)
zr = sigGen.squareWave(-1.5, 1.5, z_hz, P.Ts)
animator.background()
for _ in xrange(int(te / t_el)):
plt.pause(0.001)
for _ in xrange(int(t_el / P.Ts)):
dynams.propogateDynamics([next(hr), next(zr)])
plt.figure(animator.fig.number)
output = dynams.Outputs()
animator.draw(output, zt)
if args.save:
plt.savefig('figs/fig%d.jpg' % n)
n += 1
def plotResponse():
te = 12.0
if args.t: te = args.t
force = []
pos = []
ref = []
obs = []
hz = 0.05
z_ref = sigGen.squareWave(-0.5, 1.5, hz, P.Ts)
t = np.linspace(0.0, te, te / P.Ts)
for _ in t:
s = dynams.States()
zr = next(z_ref)
dynams.propogateDynamics([zr])
output = dynams.Outputs()
ref.append(zr)
force.append(controller.forces[0])
pos.append(output)
if args.c == 'observer':
obs.append(controller.xhat.item(0))
plt.figure(1)
plt.subplot(211)
plt.plot(t, force, 'r-')
plt.grid()
plt.title('Force')
plt.subplot(212)
plt.plot(t, ref, 'b--')
plt.plot(t, pos, 'r-')
plt.grid()
plt.title('Position of mass')
if args.c == 'observer':
plt.figure(2)
plt.subplot(211)
plt.plot(t, pos, 'r-')
plt.plot(t, obs, 'b--')
plt.grid()
plt.title('Observer vs. Actual')
plt.subplot(212)
plt.plot(t, np.array(pos) - np.array(obs), 'r-')
plt.grid()
plt.title('Error (actual - observed)')
plt.show(block=True)
def simulate():
te = 12.0
t_el = 0.1
if args.t: te = args.t
hz = 0.05
zr = sigGen.squareWave(-0.5, 1.5, hz, P.Ts)
n = 0
plt.figure(animator.fig.number)
animator.background()
for _ in xrange(int(te / t_el)):
plt.pause(0.001)
for _ in xrange(int(t_el / P.Ts)):
s = dynams.States()
dynams.propogateDynamics([next(zr)])
output = dynams.Outputs()
animator.draw(output)
if args.save:
plt.savefig('figs/fig%d.jpg' % n)
n += 1
def simulate():
global zr
te = 6.0 # Ending time in seconds
t_el = 0.1 # Time between animation updates
if args.t: te = args.t # Optional end time specification
n = 0
hz = 0.1
z_ref = sigGen.squareWave(0.1, 0.4, hz, P.Ts)
plt.figure(animator.fig.number)
animator.background()
for _ in xrange(int(te / t_el)):
plt.pause(0.001)
for _ in xrange(int(t_el / P.Ts)):
dynams.propogateDynamics([next(z_ref)])
output = dynams.Outputs()
animator.draw(output)
if args.save:
plt.savefig('figs/fig%d.jpg' % n)
n += 1
def __init__(self, controller, dynamics, animator, **kwargs): """Valid kwargs: t_end, t_elapse, Ts, signals, """ self.controller = controller self.dynamics = dynamics self.animator = animator self.t_end = 15.0 self.t_elapse = 0.1 self.Ts = 0.01 if 't_end' in kwargs: self.t_end = kwargs['t_end'] if 't_elapse' in kwargs: self.t_elapse = kwargs['t_elapse'] if 'Ts' in kwargs: self.Ts = kwargs['Ts'] if 'signal' in kwargs: self.signals = kwargs['signals'] else: hz = 0.01 self.signals = [sigGen.squareWave(-0.5, 0.5, hz, self.Ts)]
def plotResponse():
force = []
reference = []
z_resp = []
z_obs = []
theta_resp = []
theta_obs = []
te = 20.0
if args.t: te = args.t
hz = 0.1
z_ref = sigGen.squareWave(0.1, 0.4, hz, P.Ts)
t = np.linspace(0.0,te,1/P.Ts*te)
for _ in t:
zr = next(z_ref)
reference.append(zr)
dynams.propogateDynamics([zr])
output = dynams.Outputs()
z_resp.append(output[0])
theta_resp.append(output[1])
F = controller.forces[0]
force.append(F)
if args.c == 'observer':
z_obs.append(controller.xhat.item(0))
theta_obs.append(controller.xhat.item(1))
plt.figure(1)
plt.subplot(311)
plt.plot(t, force, 'b-')
plt.title('System input F')
plt.grid()
plt.subplot(312)
plt.plot(t, theta_resp, 'r-')
plt.title('System response Theta')
plt.grid()
plt.subplot(313)
plt.plot(t, reference, 'b--')
plt.plot(t, z_resp, 'r-')
plt.title('System response Z')
plt.grid()
if args.c == 'observer':
plt.figure(2)
plt.subplot(221)
plt.plot(t, z_resp, 'r-')
plt.plot(t, z_obs, 'b--')
plt.title('Actual vs Observed z')
plt.grid()
plt.subplot(222)
plt.plot(t, theta_resp, 'r-')
plt.plot(t, theta_obs, 'b--')
plt.title('Actual vs Observed theta')
plt.grid()
plt.subplot(223)
plt.plot(t, np.array(z_resp)-np.array(z_obs), 'r-')
plt.title('Error (actual - observed)')
plt.grid()
plt.subplot(224)
plt.plot(t, np.array(theta_resp)-np.array(theta_obs), 'r-')
plt.title('Error (actual - observed)')
plt.grid()
plt.show()
def plotResponse():
te = 30.0
if args.t: te = args.t
h_hz = 0.1
z_hz = 0.05
hr = sigGen.squareWave(1, 2, h_hz, P.Ts)
zr = sigGen.squareWave(-1.5, 1.5, z_hz, P.Ts)
F, tau = [], []
h_ref, h_resp = [], []
z_ref, z_resp = [], []
z_obs, h_obs = [], []
t = np.linspace(0.0, te, te / P.Ts)
for _ in t:
h_ref.append(next(hr))
z_ref.append(next(zr))
dynams.propogateDynamics([h_ref[-1], z_ref[-1] + P.wind])
forces = controller.forces
F.append(forces[0])
tau.append(forces[1])
output = dynams.Outputs()
z_resp.append(output[0])
h_resp.append(output[1])
if args.c == 'observer':
z_obs.append(controller.xhat_lat.item(0))
h_obs.append(controller.xhat_lon.item(0))
plt.figure(1)
plt.subplot(221)
plt.grid()
plt.plot(t, F, 'r-')
plt.title('Input F')
plt.subplot(223)
plt.grid()
plt.plot(t, tau, 'g-')
plt.title('Input tau')
plt.subplot(222)
plt.grid()
plt.plot(t, h_ref, 'b--')
plt.plot(t, h_resp, 'r-')
plt.title('Output h')
plt.subplot(224)
plt.grid()
plt.plot(t, z_ref, 'b--')
plt.plot(t, z_resp, 'g-')
plt.title('Output z')
if args.c == 'observer':
plt.figure(2)
plt.subplot(221)
plt.grid()
plt.plot(t, h_resp, 'r-')
plt.plot(t, h_obs, 'b--')
plt.title('Actual vs observed h')
plt.subplot(222)
plt.grid()
plt.plot(t, z_resp, 'r-')
plt.plot(t, z_obs, 'b--')
plt.title('Actual vs observed z')
plt.subplot(223)
plt.grid()
plt.plot(t, np.array(h_resp) - np.array(h_obs), 'r-')
plt.title('Error (h_actual - h_observed)')
plt.subplot(224)
plt.grid()
plt.plot(t, np.array(z_resp) - np.array(z_obs), 'r-')
plt.title('Error (z_actual - z_observed)')
plt.show(block=True)
args = parser.parse_args()
#controller = WhirlybirdControllerPID()
#controller = WhirlybirdControllerFullState()
controller = WhirlybirdControllerObserver()
dynamics = WhirlybirdDynamics(controller)
animator = WhirlybirdAnimation()
t_end = 20.0
if args.t: t_end = args.t
t_elapse = 0.1
hz = 0.05
pitch_r = 15 * np.pi / 180.0
yaw_r = 30 * np.pi / 180.0
pitch_command = sigGen.squareWave(-pitch_r, pitch_r, hz, P.Ts)
yaw_command = sigGen.squareWave(-yaw_r, yaw_r, hz, P.Ts)
pitch_ref = []
yaw_ref = []
pitch = []
yaw = []
force_left = []
force_right = []
pitch_obs = []
yaw_obs = []
plt.figure(animator.fig.number)
for _ in xrange(int(t_end / t_elapse)):
plt.pause(0.001)
for _ in xrange(int(t_elapse / P.Ts)):