% RR_XY_Force_Control
%
% RR Robot moving back and forth on the X-axis
% maintain a force of 10N while in contact
%

 t = [0:0.01:6];
 Xr = [ (1 - cos(pi*t))/2 + 0.5  ;  -0.1 + 0*t ];
 TIP = Xr;

 % tip velocity ( used for feedforward control )
 dXr = 0*Xr;
 for i=2:length(Xr)-1
    dXr(:,i) = ( Xr(:,i+1) - Xr(:,i-1) ) / 0.02;
    end

 % tip acceleration (used for feedforward control )
 ddXr = 0*Xr;
 for i=2:length(Xr)-1
    ddXr(:,i) = ( dXr(:,i+1) - dXr(:,i-1) ) / 0.02;
    end

 Q = InverseRR( [ Xr(1,1) ; 0] ) ;
 dQ = [0; 0];
 t = 0;
 dt = 0.001;
 Fy = 0;

 % Start the simulation (dt = 0.001 for stability concerns)
 Xq = [];
 Tq = [];
 Ff = [];

 for i=1:length(Xr)
    Qr = InverseRR(Xr(:,i));
    for j=1:10
       X = [ cos(Q(1)) + cos(Q(1)+Q(2));
             sin(Q(1)) + sin(Q(1)+Q(2)) ];
       J = [ -(sin(Q(1)) + sin(Q(1)+Q(2))), -sin(Q(1)+Q(2)) ;
              (cos(Q(1)) + cos(Q(1)+Q(2))), cos(Q(1)+Q(2)) ];
       dX = J * dQ;

% Control Law and Feedforward Terms
       Facc = ddXr(:,i);
       Fpid = 16*(Xr(:,i) - X) + 6*(dXr(:,i) - dX );

% Floor ( spring with k = 100 )
       Ffloor = [ 0 ; 100*(0 - X(2))  +  20*(0 - dX(2)) ];
       Ffloor = max(0, Ffloor);
      
% Control the force to 10N
       dFy = 100*(10 - Ffloor(2));
       Fy = Fy + dFy*dt;

 % gravity
       Tg = -9.8 * [ 2*cos(Q(1)) + cos(Q(1) + Q(2));  cos(Q(1) + Q(2)) ];

       T = (J') * ( Fpid + Facc + Ffloor - [ 0 ; Fy ] ) - Tg;

       ddQ = RRDynamics(Q, dQ, T);
       dQ = dQ + ddQ * dt;
       Q = Q + dQ*dt;
       t = t + dt;
       end

    RR(Q, Qr, TIP);

    Xq = [Xq, X];
    Tq = [Tq, T];
    Ff = [Ff, Ffloor];
    end

 t = [1:length(Xr)] * 0.01;
 t = min(t,6);
 clf
 subplot(211)
 plot(t,Xq,t,Xr);
 xlabel('Time (seconds)');
 ylabel('Tip (meters)');
 subplot(212)
 plot(t,Ff);
 xlabel('Time (seconds)');
 ylabel('Force (N)');