Skip to content
Snippets Groups Projects
Commit 23c4642c authored by Prince Kuevor's avatar Prince Kuevor
Browse files

Merge branch 'princeStuff'

parents 107a67dc 4269f879
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 105 additions and 64 deletions
Our Model/main.m
+ 105
− 64
View file @ 23c4642c
......@@ -22,6 +22,7 @@ rank(Co)
% it's controllable!!!! :) :) :)
%% 2) Discretize
% Ts = 0.05; % 50 Hz in ROB 550
Ts = 0.05; % 50 Hz in ROB 550
[Ad, Bd] = getDiscrete(Ac, Bc, C, D, Ts);
% sysC = ss(Ac,Bc,C,D);
......@@ -42,63 +43,65 @@ rank(Cont)
nx = length(Ad); %Number of states
nu = size(Bd,2); %Number of inputs
ne = 1; %Number of error terms
nr = 2; %Number of References
nr = 3; %Number of References
ny = size(C, 1); %Number of outputs
nxe = nx + ne; %Useful for submatrices when calling DARE
A = [Ad, zeros(nx, nu + ne + nr);
C, ones(ny, ne), zeros(ny, nu + nr);
zeros(nu, nx + ne), ones(nu, nu), zeros(nu, nr);
1, 0, 0, 0, 0, 0, 1, 0];
1, 0, 0, 0, 0, 0, 1, 0, 0];
B = [Bd;
0;
1;
0;
0];
%Adding Theta to Aug state
A = [A;
0, 0, 1, zeros(1,4), 1];
0, 0, 1, zeros(1,4), 1, 0;
0, 0, 0, 1 zeros(1,4), 1];
B = [B; 0];
%% 3.2) LQ-MPC Tracking Problem
%Weights
% Q = diag([zeros(1, nx), 1, zeros(1, nu + nr)]);
Q = diag([zeros(1, nx), 100, zeros(1, nu + 1), 0]);
% R = 20;
R = 1;
Pxe = dare(A(1:nxe, 1:nxe), B(1:nxe), ...
Q(1:nxe, 1:nxe), R);
P = blkdiag(Pxe, zeros(nu + nr));
% Q = P;
% P = Q;
%Constraints
lN = inf; %Large number (sometimes infinity doesn't work)
tMax = 0.4; %0.4;%0.2; %Maximum torque [units]
thetaMax = pi/6;
thetaMax = pi/6/2;
%State Constraints
xlim.max = [lN, lN, lN, lN, lN, tMax, lN, thetaMax];
xlim.max = [lN, lN, lN, lN, lN, tMax, lN, thetaMax, lN];
xlim.min = -xlim.max;
%Input constraints (TODO: review why this is just infinity and not tMax)
% ulim.max = lN;
ulim.max = lN;
ulim.min = -ulim.max;
%Weights
% Q = diag([zeros(1, nx), 1, zeros(1, nu + nr)]);
Q = diag([90 0 0 3000 20 zeros(1, nu + 1), 0, 0]);
% R = 20;
R = 1;
Pxe = dare(A(1:nxe, 1:nxe), B(1:nxe), ...
Q(1:nxe, 1:nxe), R);
P = blkdiag(Pxe, zeros(nu + nr));
% Q = P;
% P = Q;
%% 4) Simulation of Closed Loop Response
% Horizon and runtime
N = 30;
% Horizon
N = 10;
tFinal = 10;
%Slack Penalty
slackPen = 1e3;
slackPenMat = diag([1, 1, 1, 1, 1, 1e3, 1, 1e7]);
% slackPenMat = diag([1, 1, 1, 1, 1, 1e3, 1, 1e3, 1]);
% QP Matrices
% [H, L, G, W, T, IMPC] = formQPMatrices(A, B, Q, R, P, xlim, ulim, N);
[H, L, G, W, T, IMPC] = formQPMatricesSlack( ...
A, B, Q, R, P, xlim, ulim, N, slackPenMat);
[H, L, G, W, T, IMPC] = formQPMatrices(A, B, Q, R, P, xlim, ulim, N);
% [H, L, G, W, T, IMPC] = formQPMatricesSlack( ...
% A, B, Q, R, P, xlim, ulim, N, slackPenMat);
%
% [H, L, G, W, T, IMPC] = formQPMatrices_slack( ...
% A, B, Q, R, P, xlim, ulim, N, slackPen);
......@@ -108,7 +111,7 @@ slackPenMat = diag([1, 1, 1, 1, 1, 1e3, 1, 1e7]);
% Augmented State Initial Conditions
% x0 = [0; 0; 0; 0; 0; 0; 5/Rad; 0];
% x0 = [0; 0; 0; 0; 0; 0; 2.5/Rad; 0];
x0 = [0; 0; 0; 0; 0; 0; 0; 0];
x0 = [0; 0; 0; 0; 0; 0; 0; 0; 0];
u0 = 0;
% Trajectory Variables
......@@ -137,12 +140,10 @@ lam = ones(size(G,1),1);
% Reference Step Value
% Rad = 80 / 1000; % Radius of Wheel (was overwritten before)
step = -10;
DB = 0.0; %deadband
step = -2;
ref = zeros(1,(tFinal/Ts) + 1);
%ref(floor(2.5/Ts+1):end) = step;
ref = spline([0 1 tFinal/2 tFinal],[0 0 step 0 ],0:Ts:tFinal);
%ref = [ref, zeros(1,(1/Ts) + 1)];
ref(floor(1.0/Ts+1):end) = step;
for t=0:Ts:tFinal
% %Re-linearize
......@@ -167,7 +168,7 @@ for t=0:Ts:tFinal
q = L*x;
b = W + T*x;
[ U, lam ] = myQP(H, q, G, b, lam);
% [U] = quadprog(H, q, G, b);
% [U, out2, out3, out4, out5] = quadprog(H, q, G, b);
delta_u = IMPC*U;
......@@ -196,17 +197,19 @@ for t=0:Ts:tFinal
x(5) + Rad*(X(end,1) - x1); % new error
u; % new u
X(end,1); % new x1 (phi)
X(end,3)]; % new x3 (theta)
X(end,3);
X(end,4)]; % new x3 (theta)
% Capture the Aug State in a Traj Variable
XXX = [XXX, x*ones(1,10)];
% give step command
x(5) = Rad*x(7) - ref(floor(t/Ts + 1));
% if(t == 0.5)
% %x(5) = Rad*x(7) - ref;
% x(5) = x(5) - step;
% end
% if(t == 0.5)
% if( ~mod(t, 1))
% x(5) = Rad*x(7) - step;
% x(5) = x(5) - step;
% end
% Update actual State Variables
x1 = X(end,1);
......@@ -222,9 +225,15 @@ for t=0:Ts:tFinal
end
%% 5) Plot
figure(1)
clf
fixAxes = 1;
lw = 3;
fs = 15;
% figure(1)
figure()
clf
set(gcf, 'rend','painters','pos',[10 10 800 800])
subplot(3,2,1)
hold on;
plot(TT,rad2deg(XX(:,3)), 'linewidth', lw);
......@@ -233,37 +242,56 @@ plot([TT(1) TT(end)],-rad2deg([thetaMax thetaMax]),'r--', 'linewidth', lw);
hold off;
xlabel('Time (s)');
ylabel('\theta [deg]');%, 'fontsize', 20)
ax = gca;
ax.XLim = [0 10];
grid;
if fixAxes
ax = gca;
ax.XLim = [0 10];
ax.XMinorTick = 'on';
ax.YMinorTick = 'on';
end
set(gca,'fontsize',fs)
% legend('\theta','\theta dot');
subplot(3,2,2)
plot(TT,rad2deg(XX(:,4)), 'linewidth', lw);
xlabel('Time (s)');
ylabel('\theta dot [deg/s]');%, 'fontsize', 20)
ax = gca;
ax.XLim = [0 10];
ylabel('$\dot{\theta}$ [deg/s]','Interpreter', 'latex');
grid;
if fixAxes
ax = gca;
ax.XLim = [0 10];
ax.XMinorTick = 'on';
ax.YMinorTick = 'on';
end
set(gca,'fontsize',fs)
% subplot(3,2,3)
% plot(TT,XX(:,1), 'linewidth', lw);
% xlabel('Time (s)');
% grid;
% ylabel('\phi [rad]');%, 'fontsize', 20)
% if fixAxes
% ax = gca;
% ax.XLim = [0 10];
% end
subplot(3,2,3)
plot(TT,XX(:,1), 'linewidth', lw);
xlabel('Time (s)');
ylabel('\phi [rad]');%, 'fontsize', 20)
ax = gca;
ax.XLim = [0 10];
grid;
subplot(3,2,4)
plot(TT,XX(:,2), 'linewidth', lw);
plot(TT,Rad*XX(:,2), 'linewidth', lw);
xlabel('Time (s)');
ylabel('\phi dot [rad/s]');%, 'fontsize', 20)
ax = gca;
ax.XLim = [0 10];
grid;
% ylabel('\phi dot [rad/s]');%, 'fontsize', 20)
ylabel('Velocity [m/s]');%, 'fontsize', 20)
if fixAxes
ax = gca;
ax.XLim = [0 10];
ax.XMinorTick = 'on';
ax.YMinorTick = 'on';
end
set(gca,'fontsize',fs)
subplot(3,2,5)
subplot(3,2,3)
hold on;
% plot(TT,XXX(6,:), 'linewidth', lw);
plot((1:10:size(XXX,2))*h, XXX(6,1:10:end), 'linewidth', lw);
......@@ -271,23 +299,36 @@ plot([TT(1) TT(end)],[tMax tMax],'r--', 'linewidth', lw);
plot([TT(1) TT(end)],-[tMax tMax],'r--', 'linewidth', lw);
hold off;
xlabel('Time (s)');
ylabel('Control Input');%, 'fontsize', 20)
ax = gca;
ax.XLim = [0 10];
grid;
ylabel('\tau [N-m]');%, 'fontsize', 20)
if fixAxes
ax = gca;
ax.XLim = [0 10];
ax.XMinorTick = 'on';
ax.YMinorTick = 'on';
end
set(gca,'fontsize',fs)
subplot(3,2,6)
subplot(3,1,3)
hold on;
plot(TT,Rad*XX(:,1), 'linewidth', lw);
%plot([TT(1) TT(end)],[step step] + Rad*x0(7),'k--', 'linewidth', lw);
plot(0:Ts:tFinal,ref(1:floor(tFinal/Ts+1)), 'k--', 'linewidth', lw);
plot(0:Ts:tFinal,ref,'k--', 'linewidth', lw);
hold off;
xlabel('Time (s)');
ylabel('Position [m]')
ax = gca;
ax.XLim = [0 10];
ax.YLim = [-4 1];
grid;
if fixAxes
ax = gca;
ax.XLim = [0 10];
ax.YLim = [-11 1];
ax.XMinorTick = 'on';
ax.YMinorTick = 'on';
end
set(gca,'fontsize',fs)
%%
% figure(2);
......@@ -301,7 +342,7 @@ grid;
%% 6) Animate
% Segway_anim(t,phi,theta); phi is pend angle, theta is wheel angle
%Segway_anim(TT,XX(:,3),XX(:,1),0.1);
Segway_anim(TT,XX(:,3),XX(:,1),0.1);
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment