Added functionality to follow an arbitrary setpoint.

Our Model/main.m

@@ -62,7 +62,7 @@ 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), 1/80, zeros(1, nu + 1), 1/2]);
+Q = diag([zeros(1, nx), 100, zeros(1, nu + 1), 0]);
 
 % R = 20;
 R = 1;
@@ -86,8 +86,9 @@ ulim.max = lN;
 ulim.min = -ulim.max;
 
 %% 4) Simulation of Closed Loop Response
-% Horizon
+% Horizon and runtime
 N = 30;
+tFinal = 10;
 
 %Slack Penalty
 slackPen = 1e3;
@@ -136,11 +137,14 @@ lam = ones(size(G,1),1);
 
 % Reference Step Value
 % Rad = 80 / 1000;    % Radius of Wheel (was overwritten before)
-step = -5;
 DB = 0.0;   %deadband
-% ref = -5;
+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)];
 
-for t=0:Ts:10
+for t=0:Ts:tFinal
 %     %Re-linearize
 %     [Ac,Bc] = symLinFast([x1, x2, x3, x4]',u);
 %     
@@ -198,10 +202,11 @@ for t=0:Ts:10
      XXX = [XXX, x*ones(1,10)];
      
      % give step command
-     if(t == 0.5)
-%         x(5) = Rad*x(7) - ref;
-        x(5) = x(5) - step;
-     end
+     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
     
     % Update actual State Variables
     x1 = X(end,1);
@@ -274,13 +279,14 @@ grid;
 subplot(3,2,6)
 hold on;
 plot(TT,Rad*XX(:,1), 'linewidth', lw);
-plot([TT(1) TT(end)],[step step] + Rad*x0(7),'k--', '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);
 hold off;
 xlabel('Time (s)');
 ylabel('Position [m]')
 ax = gca;
 ax.XLim = [0 10];
-ax.YLim = [-5 5];
+ax.YLim = [-4 1];
 grid;
 
 %% 
@@ -295,7 +301,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.02);
+%Segway_anim(TT,XX(:,3),XX(:,1),0.1);