% RR_Wall_Control.txt
% Position control of a RR robot
% similar to a RRR robot with Q1 = 0, meaning Y=0
%
% Define a square to trace
disp('Defining Path to Follow');
	P1 = [0.5, 0]';
	P2 = [1.5, 0]';
	P3 = [1.5, 1]';
	P4 = [0.5, 1]';
	P5 = P1;

disp('Calculating tip positions');
% Determine the tip positions every 10ms
	T1 = Spline1(P1,P2,1);
	T2 = Spline1(P2,P3,1);
	T3 = Spline1(P3,P4,1);
	T4 = Spline1(P4,P5,1);
	TIP = [T1,T2,T3,T4];

disp('Calculating joint angles');
% Determie the joint angles every 10ms
	Qr = [];
	for i=1:length(TIP)
		q = InverseRR(TIP(:,i));
		Qr = [Qr, q];
		end
	c1 = cos(Qr(1,:));
	s1 = sin(Qr(1,:));
	c2 = cos(Qr(2,:));
	s2 = sin(Qr(2,:));
	c12 = cos(Qr(1,:) + Qr(2,:));
	s12 = sin(Qr(1,:) + Qr(2,:));

disp('Calulating gravity matrix');
% gravity
	g = 9.8;
	G = g*[3*c1 + c12 ; c12 ];

disp('Calulating gravity torques');
% Velocity
	dQr1 = Derivative(Qr(1,:));
	dQr2 = Derivative(Qr(2,:));
	dQr = [dQr1 ; dQr2];

disp('Calulating coriolis torques');
% Coriolis Forces
	C = [ 2*s2.*dQr1.*dQr2 + s2.*dQr2.*dQr2 ; -s2.*dQr1.*dQr1 ];

disp('Calulating inertia torques');
% Acceleration
	ddQr1 = Derivative(dQr(1,:));
	ddQr2 = Derivative(dQr(2,:));
	ddQr = [ddQr1 ; ddQr2];

% Inertia
	M = [];
	for i=1:length(Qr)
		M = [M, [4+2*c2(i), 1+c2(i) ; 1+c2(i), 1]*ddQr];
    end

    Q = Qr(:,1);
	dQ = [0; 0];
	T = [0; 0];
	t = 0;
	dt = 0.01;
	TIP = [];
	TIPr = [];
    Torque = [];
    Force = [];
    

disp('Tracing out a Square');
	for i=1:length(Qr)
		T1 = 200*(Qr(1,i) - Q(1)) + 40*(dQr(1,i) - dQ(1));
		T2 = 200*(Qr(2,i) - Q(2)) + 40*(dQr(2,i) - dQ(2));
		T = [T1; T2];
% plus gravity
		T = T - G(:,i);

% plus derivative
% (already in the T1 and T2 equations )

% plus coriolis
		T = T - C(:,i);

% plus acceleration
		c2 = cos(Q(2));
		T = T + [4+2*c2, 1+c2 ; 1+c2, 1]*ddQr(:,i);
 
% Jacobian
       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;
       
% plus wall at X = 1.30
 
       Fwall = [ min(0,  10000*(1.3 - x2) - 140*dX(1) );  0];


       T = T + J'*Fwall;
       Torque = [Torque, T];   
       Force = [Force, Fwall]; 
        
		ddQ = RR_Dynamics(Q, dQ, T);
		dQ = dQ + ddQ * dt;
		Q = Q + dQ*dt;
		t = t + dt;

% Robot
		x0 = 0;
		y0 = 0;
		x1 = cos(Q(1));
		y1 = sin(Q(1));
		x2 = x1 + cos(Q(1) + Q(2));
		y2 = y1 + sin(Q(1) + Q(2));
		TIP = [TIP, [x2 ; y2]];

% Reference Point
		xr0 = 0;
		yr0 = 0;
		xr1 = cos(Qr(1,i));
		yr1 = sin(Qr(1,i));
		xr2 = xr1 + cos(Qr(1,i) + Qr(2,i));
		yr2 = yr1 + sin(Qr(1,i) + Qr(2,i));
		TIPr = [TIPr, [xr2 ; yr2]];
		clf;
		plot([-0.5,2],[-0.5,2],'x');
		hold on;
		plot([-0.5,2],[0,0], 'r');
		plot([0,0],[-0.5,2], 'r');
		plot([x0, x1, x2], [y0, y1, y2], 'b.-', [xr0, xr1, xr2], [yr0, yr1, yr2], 'c.-');
		plot(TIPr(1,:),TIPr(2,:),'g', TIP(1,:),TIP(2,:),'m');
        plot([1.3,1.3],[0,1.5],'r--');
		pause(0.01);
    end
        
    
    pause(5);
    
clf
t = [1:length(Force)] * 0.01;
plot(t,Force)