% RR_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,2);
	T2 = Spline1(P2,P3,2);
	T3 = Spline1(P3,P4,2);
	T4 = Spline1(P4,P5,2);
	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;
    TIPxy = [];

disp('Tracing out a Square');
	for i=1:length(Qr)
		T1 = 160*(Qr(1,i) - Q(1)) + 40*(0*dQr(1,i) - dQ(1));
		T2 = 32*(Qr(2,i) - Q(2)) + 8*(0*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);
		ddQ = RR_Dynamics(Q, dQ, T);
		dQ = dQ + ddQ * dt;
		Q = Q + dQ*dt;
		t = t + dt;

        T = RR(Q, Qr(:,i), TIP);
        TIPxy = [TIPxy, T];
        plot(TIPxy(1,:),TIPxy(2,:),'c');
    end