close all;

clear all;
clc;

% Caracteristicas del BR
L1 = 10;
L2 = 7;
dt = 4;
t1 = 1:dt:80;
t2 = -70:dt:70;

% Calculo del espacio de trabajo del BR (Cinematica Directa)

[T1, T2] = meshgrid(t1,t2);
X1 = L1*cosd(T1);
Y1 = L1*sind(T1);
X = X1+L2*cosd(T1+T2);
Y = Y1+L2*sind(T1+T2);

% Representacion del LM del BR

plot(X1,Y1,'k.',X,Y,'g.');
vs = [-60 60 -60 60];
line([-60 60],[0 0],'Color','r');
line([0 0],[-60 60],'Color','r');
axis(vs);
grid on;
axis square;
title('Lugar de movimiento del BR');
xlabel('x, cm');
ylabel('y, cm');

% Definicion de la trayectoria de aplicacion r1:pA-pB

pA = [10,12];

N = 20;
m = (pB(2)-pA(2))/(pB(1)-pA(1));
y0 = pA(2) - m*pA(1);
x = linspace(pA(1),pB(1),N);
y = m*x+y0;

% Calculo de la Cinematica Inversa para la recta "r1"

LL = L1^2 + L2^2;
r2 = x.^2 + y.^2;
cs2 = (r2 - LL)/(2*L1*L2);
sn2 = sqrt(1 - cs2.^2);
a = L1 + L2*cs2;
b = L2*sn2;
t1i = 180*(atan2(y,x) - atan2(b,a))/pi;
t2i = 180*atan2(sn2,cs2)/pi;

% Tabla solucion o base de datos de la CI

TS = [x; y; t1i; t2i]';

% Representacion grafica de la CI
x1 = L1*cosd(t1i);
y1 = L1*sind(t1i);

hold on

plot(0,0,'ko','MarkerFaceColor','k');
line([pA(1) pB(1)],[pA(2) pB(2)],'LineWidth',2);

for k=1:N
line([0 x1(k)],[0 y1(k)],'Color','m','LineWidth',2);
plot(x1(k),y1(k),'mo','MarkerFaceColor','m');
line([x1(k) x(k)],[y1(k) y(k)],'Color','k','LineWidth',2);
plot(x(k),y(k),'bo','MarkerFaceColor','b');
pause(1);
end

hold off

ls
edit cininv
cininv
TS
dt1i = TS(:,3)
dt1i = TS(2:end,3)-TS(1:end-1,3)
figure; plot(dt1i); grid on;
title('\Delta\theta_1, DEG')
dt2i = TS(2:end,4)-TS(1:end-1,4)
figure; plot(dt2i); grid on;
title('\Delta\theta_2, DEG')
dt2i = round(TS(2:end,4)-TS(1:end-1,4))
dt2i = TS(2:end,4)-TS(1:end-1,4)