100 lines
2.1 KiB
Matlab
100 lines
2.1 KiB
Matlab
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace
|
|
|
|
%code snipplets from an example on youtube (see reference at top)
|
|
function SCRATCH()
|
|
%import numpy as np
|
|
%fh=np.load('mode1.npz')
|
|
%import scipy.io
|
|
%scipy.io.savemat('mode1.mat',fh,do_compression=True)
|
|
|
|
%matlab:
|
|
load('mode1.mat');
|
|
plot(pts(:,1),pts(:,2),'.');hold on;
|
|
plot(rec(:,5),rec(:,6),'-');%despos
|
|
plot(rec(:,2),rec(:,3),'-');%actPos
|
|
|
|
%sig.time = [0 1 1 5 5 8 8 10];
|
|
%sig.signals.values = [0 0 2 2 2 3 3 3]';
|
|
%sig.signals.dimensions = 1;
|
|
sig.time=0:2E-4:(length(rec)-1)*2E-4;
|
|
sig.signals.values=rec(:,5);
|
|
sig.signals.dimensions = 1;
|
|
|
|
sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2))
|
|
|
|
A = [ 0 1 0
|
|
980 0 -2.8
|
|
0 0 -100 ];
|
|
|
|
B = [ 0
|
|
0
|
|
100 ];
|
|
|
|
C = [ 1 0 0 ];
|
|
|
|
poles = eig(A)
|
|
|
|
t = 0:0.01:2;
|
|
u = zeros(size(t));
|
|
x0 = [0.01 0 0];
|
|
|
|
sys = ss(A,B,C,0);
|
|
|
|
[y,t,x] = lsim(sys,u,t,x0);
|
|
plot(t,y)
|
|
title('Open-Loop Response to Non-Zero Initial Condition')
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
|
|
p1 = -10 + 10i;
|
|
p2 = -10 - 10i;
|
|
p3 = -50;
|
|
|
|
K = place(A,B,[p1 p2 p3]);
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
|
|
lsim(sys_cl,u,t,x0);
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
|
|
p1 = -20 + 20i;
|
|
p2 = -20 - 20i;
|
|
p3 = -100;
|
|
|
|
K = place(A,B,[p1 p2 p3]);
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
|
|
lsim(sys_cl,u,t,x0);
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
|
|
|
|
t = 0:0.01:2;
|
|
u = 0.001*ones(size(t));
|
|
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
|
|
lsim(sys_cl,u,t);
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
axis([0 2 -4E-6 0])
|
|
|
|
Nbar = rscale(sys,K)
|
|
|
|
lsim(sys_cl,Nbar*u,t)
|
|
title('Linear Simulation Results (with Nbar)')
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
axis([0 2 0 1.2*10^-3])
|
|
end
|
|
|
|
clear;clear global;close all;
|
|
mot=identifyFxFyStage(7);
|
|
for k =1:2
|
|
[pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
|
|
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
|
|
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
|
|
print(f,sprintf('figures/sim_cl_DTGz_%d',mot{k}.id),'-depsc');
|
|
end
|
|
|