
omega1=1; omega2=sqrt(2);
%a=-5; b=2; c=0;
a=-2; b=0; c=0;
mu=-0.5;
l1=1; l2=1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q1_0=1/2*pi/2;           % Set initial position for q1(t) and q2(t)
q2_0=0.5;

x0=[q1_0-pi/2  0  q2_0 0];
step=0.1;           % Set step time simulation
tf=15;              % Set time simulation
t = 0:step:tf;
revers=1;           % [1 or -1] Run animation at invers time 
[t,X] = ode23s(@(t,x) TwoLinkRobotEulerianControled(x,mu, omega1, omega2, a, b, c),revers*t,x0);
X1=X(:,1);
X2=X(:,3);
axis off;
for i=1:length(t)
    Theta1=X1(i)+pi/2;
    Theta2=X2(i);
   
S=[0,l1*cos(Theta1), l1*cos(Theta1)+l2*cos(Theta1+Theta2)];
T=[0,l1*sin(Theta1),  l1*sin(Theta1)+l2*sin(Theta1+Theta2) ];

x1=0*S; y1=0*T;
y2=l1*sin(Theta1)*ones(1,3);
x2=l1*cos(Theta1)*ones(1,3);

 xlim([-2 2]);
 ylim([-2 2.3]);

pause(0.001);       % Set speed of animation
clf
 p=plot(S, T, x1, y1,'-o',x2, y2, '-o', 'LineWidth', 5);
     %%%%%   Dot notation  is used for  (at least)  Matlab R2014b, R2015 , ... 
%  p(1).LineWidth = 2;            
%  p(2).MarkerSize = 8; p(2).LineWidth = 1.5; p(2).MarkerFaceColor ='red';p(2).MarkerEdgeColor='none';                      
%  p(3).MarkerSize = 8; p(3).LineWidth = 1.5; p(3).MarkerFaceColor ='red';p(3).MarkerEdgeColor='none';

  set(p(1),'LineWidth',3);     set(p(1),'Color','black');
  set(p(2),'MarkerSize',10);   set(p(2),'MarkerFaceColor','blue'); set(p(2),'MarkerEdgeColor', 'none');
  set(p(3),'MarkerSize',10);   set(p(3),'MarkerFaceColor','blue'); set(p(3),'MarkerEdgeColor', 'none');
  
axis equal; 
xlim([-2 2]);
 ylim([-2 2.3]);

set(gca,'FontSize',10);
set(gca,'FontAngle','Italic ');
set(gca,'FontWeight','normal');
ax=gca;
axis(ax, 'off');  % Set axis 'on' or 'off' 

str = ['Time=',sprintf('%.1f , ' ,t(i)),'q_1=', sprintf('%.2f',mod(Theta1*180/pi,360)),...
                                            '^{\circ}',', q_2=',sprintf('%.2f',mod(Theta2*180/pi, 360)),'^{\circ}' ];

xlabel(ax,str);
%ax.XLabel.Visible='on';
s=get(ax,'Xlabel' );
set(s , 'Visible', 'on');
title(sprintf('Two Link Robot'),'FontSize',15');
f=gcf;
%f.MenuBar='none';
set(f, 'MenuBar','none');
x0=3.5;    y0=1;
width=7;  height=6;
set(f,'units','inches','position',[x0 y0 width height]);

end
