% RR_Control with Gears.txt
% Position control of a RR robot
% Assumes gear reduction so that only the motor dynamics matter

% 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
    
disp('Motor Dynamics');
    Gm = tf(100, [1,10,100]);
    
disp('Simulating motor dynamics');
    Qr1 = Qr(1,:)';
    Qr2 = Qr(2,:)';
    t = [1:length(Qr1)]' * 0.01;

    Q1 = step2(Gm, t, Qr1);
    Q2 = step2(Gm, t, Qr2);
    
    

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

disp('Tracing out a Square');
	for i=1:length(Qr)
        Q(1) = Q1(i);
        Q(2) = Q2(i);
    	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');
		pause(0.01);
		end