Subtle Changes. Added functionality for arbitrary reference and added variable for tFinal.

Our Model/main.m

@@ -91,6 +91,7 @@ P = blkdiag(Pxe, zeros(nu + nr));
 %% 4) Simulation of Closed Loop Response
 % Horizon
 N = 10;
+tFinal = 10;
 
 %Slack Penalty
 slackPen = 1e3;
@@ -141,9 +142,10 @@ lam = ones(size(G,1),1);
 % Rad = 80 / 1000;    % Radius of Wheel (was overwritten before)
 step = -10;
 DB = 0.0;   %deadband
-% ref = -5;
+ref = zeros(1,(tFinal/Ts) + 1);
+ref(floor(1.0/Ts+1):end) = step;
 
-for t=0:Ts:15
+for t=0:Ts:tFinal
 %     %Re-linearize
 %     [Ac,Bc] = symLinFast([x1, x2, x3, x4]',u);
 %     
@@ -202,11 +204,12 @@ for t=0:Ts:15
      XXX = [XXX, x*ones(1,10)];
      
      % give step command
-     if(t == 0.5)
+     x(5) = Rad*x(7) - ref(floor(t/Ts + 1));
+%     if(t == 0.5)
 %      if( ~mod(t, 1))
 %         x(5) = Rad*x(7) - step;
-        x(5) = x(5) - step;
-     end
+%        x(5) = x(5) - step;
+%     end
     
     % Update actual State Variables
     x1 = X(end,1);
@@ -222,7 +225,7 @@ for t=0:Ts:15
 end
 
 %% 5) Plot
-fixAxes = 0;
+fixAxes = 1;
 
 % figure(1)
 figure()
@@ -295,7 +298,8 @@ end
 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,'k--', 'linewidth', lw);
 hold off;
 xlabel('Time (s)');
 ylabel('Position [m]')
@@ -303,7 +307,7 @@ grid;
 if fixAxes
     ax = gca;
     ax.XLim = [0 10];
-    ax.YLim = [-5 5];
+    ax.YLim = [-11 1];
 end
 
 
@@ -320,7 +324,7 @@ end
 
 %% 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);