clear all

close all

ts=0.1;
t=0:ts:20;

u=0*ones(1,length(t));
w=0.5*ones(1,length(t));

xr(1)=0;
yr(1)=0;
phi(1)=0;

for k=1:length(t)

xrp(k)=u(k)*cos(phi(k));
yrp(k)=u(k)*sin(phi(k));

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
phi(k+1)=phi(k)+ts*w(k);

end

pasos=20; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
axis([-8 10 -6 6 0 1]); grid on
MobileRobot;
M1=MobilePlot(xr(1),yr(1),phi(1));
hold on, plot(xr,yr);

for i=1:pasos:length(t)

delete (M1)
M1=MobilePlot(xr(i),yr(i),phi(i)); hold on

pause(0.5)

end
_________________________________________________________________________
_________________________________________________________________________
*************************************************************************
*************************************************************************
clc
clear all
close all

ts=0.1;
t=0:ts:20;

u=0*ones(1,length(t));
w=0.5*ones(1,length(t));

a=0.2;

xc(1)=0;
yc(1)=0;
phi(1)=0;

xr(1)=xc(1)+a*cos(phi(1));
yr(1)=yc(1)+a*sin(phi(1));

for k=1:length(t)
 xrp(k)=u(k)*cos(phi(k))-a*w(k)*sin(phi(k));
yrp(k)=u(k)*sin(phi(k))+a*w(k)*cos(phi(k));

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
phi(k+1)=phi(k)+ts*w(k);

xc(k+1)=xr(k+1)-a*cos(phi(k+1));
yc(k+1)=yr(k+1)-a*sin(phi(k+1));

end

pasos=20; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
axis([-8 10 -6 6 0 1]); grid on
MobileRobot;
M1=MobilePlot(xr(1),yr(1),phi(1));
hold on, plot(xr,yr);

for i=1:pasos:length(t)

delete (M1)
M1=MobilePlot(xc(i),yc(i),phi(i)); hold on

pause(0.5)

end
clc
clear all
close all

ts=0.1;
t=0:ts:60;

% 1. Condiciones iniciales del manipulador

l1=0.5;
l2=0.65;
h=0.25;

q1(1) = 0*(pi/180);
q2(1) = 30*(pi/180);
q3(1) = 30*(pi/180);

xr(1)=l1*cos(q2(1))*cos(q1(1))+l2*cos(q2(1)+q3(1))*cos(q1(1));
yr(1)=l1*cos(q2(1))*sin(q1(1))+l2*cos(q2(1)+q3(1))*sin(q1(1));
zr(1)=h+l1*sin(q2(1))+l2*sin(q2(1)+q3(1));

% 3) Referencias deseadas
% xrd = 0.5;
% yrd = -0.8;
% zrd = 0.2;
 xrd = .6*cos(0.2*t);
yrd = .6*sin(0.2*t);
zrd = 0.7*ones(1,length(t));
% zrd =0.7+0.01*t;

xrd_p= -.6*0.2*sin(0.2*t);
yrd_p= .6*0.2*cos(0.2*t);
% zrd_p = 0.01*ones(1,length(t));

zrd_p= 0*ones(1,length(t));

for k=1:length(t)

%a) Errores de control

xre(k) = xrd(k) - xr(k);
yre(k) = yrd(k) - yr(k);
zre(k) = zrd(k) - zr(k);

e = [xre(k);yre(k);zre(k)];

%b) Matriz Jacobiana

J=[ -l1*cos(q2(k))*sin(q1(k))-l2*cos(q2(k)+q3(k))*sin(q1(k)) -
l1*cos(q1(k))*sin(q2(k))-l2*cos(q1(k))*sin(q2(k)+q3(k)) -
l2*cos(q1(k))*sin(q2(k)+q3(k));...
l1*cos(q2(k))*cos(q1(k))+l2*cos(q2(k)+q3(k))*cos(q1(k)) -
l1*sin(q1(k))*sin(q2(k))-l2*sin(q1(k))*sin(q2(k)+q3(k)) -
l2*sin(q1(k))*sin(q2(k)+q3(k));...
0 l1*cos(q2(k))+l2*cos(q2(k)+q3(k)) l2*cos(q2(k)+q3(k))];

%c) Matriz de ganancia

K = [0.2 0 0;...
0 0.2 0;...
0 0 0.2];

%d) Ley de control

% v = inv(J)*K*e;

hd_p=[xrd_p(k) yrd_p(k) zrd_p(k)]';

%d) Ley de control

v = inv(J)*(hd_p+K*tanh(e));

q1p_ref(k)=v(1);
q2p_ref(k)=v(2);
q3p_ref(k)=v(3);

q1(k+1)=q1(k)+q1p_ref(k)*ts;
q2(k+1)=q2(k)+q2p_ref(k)*ts;
q3(k+1)=q3(k)+q3p_ref(k)*ts;
xr(k+1)=l1*cos(q2(k+1))*cos(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*cos(q1(k+1))
;

yr(k+1)=l1*cos(q2(k+1))*sin(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*sin(q1(k+1))
;
zr(k+1)=h+l1*sin(q2(k+1))+l2*sin(q2(k+1)+q3(k+1));
end

pasos=5; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
axis([-1 1 -1 1 0 3]);

Arm_Parameters;

M1=Arm_Plot3D(0,0,0,0,0,q1(1),q2(1),q3(1),0);

for i=1:pasos:length(t)

delete (M1)
M1=Arm_Plot3D(0,0,0,0,0,q1(i),q2(i),q3(i),0),hold on;
plot3(xr(1:i),yr(1:i),zr(1:i),'r','linewidth',3);
pause(0.1)

end

=============================================================
clc % Clear Command Window
clear all %Remove items from workspace, freeing up system memory
close all % Remove specified figure

ts=0.1; % sample time

t=0:ts:30; % vector time

a=0.3;
% initial center position of robot
hx(1)=0; % X[m]
hy(1)=0; % Y[m]
phi(1)=0; % [rad]

% 3) Desired reference
hxd = 2*cos(0.2*t);
hyd = 2*sin(0.2*t);

hxdp = -2*0.2*sin(0.2*t);
hydp = 2*0.2*cos(0.2*t);

K=1;

hx(1)=hx(1)+a*cos(phi(1));
hy(1)=hy(1)+a*sin(phi(1));
for k=1:length(t)

% Following trayectory Control

% Error
hxe(k)=hxd(k)-hx(k);
hye(k)=hyd(k)-hy(k);

e=[hxe(k) hye(k)]';

% Jacobian Matrix

J=[cos(phi(k)) -sin(phi(k));...
sin(phi(k)) cos(phi(k))];

% Desired velocities
hdp=[hxdp(k) hydp(k)]';

v=inv(J)*(hdp+(K*e));

uf(k)=v(1);
ul(k)=v(2);

% Set action control in robot(real) or model

hxp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k));
hyp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k));

% Position in k+1
hx(k+1)=hx(k)+ts*hxp(k);
hy(k+1)=hy(k)+ts*hyp(k);
phi(k+1)=phi(k);
end

%% Simulacion

fig=figure; % new figure

set(fig,'position',[10 60 980 600]); % position window
axis equal; % Set axis aspect ratios
axis([-3 3 -3 3 -1 1]); % Set axis limits
grid on; % Display axes grid lines

Dimension_Omni; % Parameters of robot

M1=Plot_Omni(hx(1),hy(1),phi(1)); % Plot robot in initial position hx,hy
and phi orientation
hold on; % Retain current plot when adding new plot
plot(hxd,hyd,'r'); % plot trayectory.
xlabel('x(m)'); ylabel('y(m)'); zlabel('z(x)'); % Label axis
camlight('rigth');
%%
step=5;

for i=1:step:length(t) % Loop emulation

delete (M1)
M1=Plot_Omni(hx(i),hy(i),phi(i)); hold on
hold on; % Retain current plot when adding new plot
plot(hx(1:i),hyd(1:i),'b'); % plot trayectory.
pause(ts)
end
=========================================================================
0==

OMNIDIRECCIONAL_____CON VELOCIDAD RUEDAS

clc
clear all
close all

% Time
ts=0.1;
t=0:ts:20;

uf=-0.3*ones(1,length(t));
ul=0.3*ones(1,length(t));
w=0.0*ones(1,length(t));

a=0.15;
b=0.10;
R=0.02;

T=(1/R)*[1 -1 -(a+b);...
1 1 (a+b);...
1 1 -(a+b);...
1 -1 (a+b)];

xr(1)=0;
yr(1)=0;
phi(1)=0;

for k=1:length(t)

xrp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k));
yrp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k));

Wd=T*([uf(k);ul(k);w(k)]);

Wd1(k)=Wd(1);
Wd2(k)=Wd(2);
Wd3(k)=Wd(3);
 Wd4(k)=Wd(4);

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
phi(k+1)=phi(k)+ts*w(k);

end

pasos=10; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
% axis([-3 3 -3 3 0 1]); grid on
DimensionOmni;
M1=Plot_Omni(xr(1),yr(1),phi(1));
hold on, plot(xr,yr,'g','linewidth',2);

for i=1:pasos:length(t)

delete (M1)
M1=Plot_Omni(xr(i),yr(i),phi(i)); hold on

pause(0.1)

end

OMNIDIRECCIONAL
=============================================================
==========
DRONE
clc
clear all
close all

ts=0.1;
t=0:ts:10;
a=0.1;

% 1. Condiciones iniciales
xc(1) = 0; %pocisión en el eje x en (m)
yc(1) = 0; %pocisión en el eje y en (m)
zr(1) = 0; %pocisión en el eje z en (m)

phi(1)= 0*(pi/180); %orientación en radianes

xr(1)=xc(1)+a*cos(phi(1));
yr(1)=yc(1)+a*sin(phi(1));

% 3) Referencias deseadas
xrd = -2;
yrd = 2;
zrd = 5;
phid=90*(pi/180);

for k=1:length(t)

%a) Errores de control

xre(k) = xrd - xr(k);
yre(k) = yrd - yr(k);
zre(k) = zrd - zr(k);
phie(k) = phid-phi(k);
 e = [xre(k);yre(k);zre(k);phie(k)];

%b) Matriz Jacobiana

J=[cos(phi(k)) -sin(phi(k)) 0 -a*sin(phi(k));...
sin(phi(k)) cos(phi(k)) 0 a*cos(phi(k));...
0 0 1 0;...
0 0 0 1];

%c) Matriz de ganancia

K = [0.1 0 0 0;...
0 0.1 0 0;...
0 0 0.5 0;...
0 0 0 0.5];

%d) Ley de control

v = inv(J)*K*e;

uf(k)=v(1);
ul(k)=v(2);
uz(k)=v(3);
w(k)=v(4);

xrp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k))-a*w(k)*sin(phi(k));
yrp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k))+a*w(k)*cos(phi(k));
zrp(k)=uz(k);

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
zr(k+1)=zr(k)+ts*zrp(k);

phi(k+1)=phi(k)+ts*w(k);

xc(k+1)=xr(k+1)-a*cos(phi(k+1));
yc(k+1)=yr(k+1)-a*sin(phi(k+1));

end

pasos=5; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
axis([-3 3 -3 3 0 7]); grid on

M1=Drone6(xc(1),yc(1),zr(1),0,0,phi(1),1);

hold on, plot3(xr,yr,zr);

for i=1:pasos:length(t)

delete (M1)
M1=Drone6(xc(i),yc(i),zr(i),0,0,phi(i),1);

pause(0.1)

end

=============================================================
============ BRAZO ROBOTICO
clc
clear all
close all

ts=0.1;
t=0:ts:60;

% 1. Condiciones iniciales del manipulador

l1=0.5;
l2=0.65;
h=0.25;

q1(1) = 0*(pi/180);
q2(1) = 30*(pi/180);
q3(1) = 30*(pi/180);

xr(1)=l1*cos(q2(1))*cos(q1(1))+l2*cos(q2(1)+q3(1))*cos(q1(1));
yr(1)=l1*cos(q2(1))*sin(q1(1))+l2*cos(q2(1)+q3(1))*sin(q1(1));
zr(1)=h+l1*sin(q2(1))+l2*sin(q2(1)+q3(1));

% 3) Referencias deseadas
% xrd = 0.5;
% yrd = -0.8;
% zrd = 0.2;

xrd = .6*cos(0.2*t);
yrd = .6*sin(0.2*t);
zrd = 0.7*ones(1,length(t));
% zrd =0.7+0.01*t;

xrd_p= -.6*0.2*sin(0.2*t);
yrd_p= .6*0.2*cos(0.2*t);
% zrd_p = 0.01*ones(1,length(t));

zrd_p= 0*ones(1,length(t));

for k=1:length(t)

%a) Errores de control

 xre(k) = xrd(k) - xr(k);
yre(k) = yrd(k) - yr(k);
zre(k) = zrd(k) - zr(k);

e = [xre(k);yre(k);zre(k)];

%b) Matriz Jacobiana

J=[ -l1*cos(q2(k))*sin(q1(k))-l2*cos(q2(k)+q3(k))*sin(q1(k)) -
l1*cos(q1(k))*sin(q2(k))-l2*cos(q1(k))*sin(q2(k)+q3(k)) -
l2*cos(q1(k))*sin(q2(k)+q3(k));...
l1*cos(q2(k))*cos(q1(k))+l2*cos(q2(k)+q3(k))*cos(q1(k)) -
l1*sin(q1(k))*sin(q2(k))-l2*sin(q1(k))*sin(q2(k)+q3(k)) -
l2*sin(q1(k))*sin(q2(k)+q3(k));...
0 l1*cos(q2(k))+l2*cos(q2(k)+q3(k)) l2*cos(q2(k)+q3(k))];

%c) Matriz de ganancia

K = [0.2 0 0;...
0 0.2 0;...
0 0 0.2];

%d) Ley de control

% v = inv(J)*K*e;

hd_p=[xrd_p(k) yrd_p(k) zrd_p(k)]';

%d) Ley de control

v = inv(J)*(hd_p+K*tanh(e));

q1p_ref(k)=v(1);
q2p_ref(k)=v(2);
q3p_ref(k)=v(3);

q1(k+1)=q1(k)+q1p_ref(k)*ts;
q2(k+1)=q2(k)+q2p_ref(k)*ts;
q3(k+1)=q3(k)+q3p_ref(k)*ts;

xr(k+1)=l1*cos(q2(k+1))*cos(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*cos(q1(k+1))
;

yr(k+1)=l1*cos(q2(k+1))*sin(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*sin(q1(k+1))
;
zr(k+1)=h+l1*sin(q2(k+1))+l2*sin(q2(k+1)+q3(k+1));
end

pasos=5; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
axis([-1 1 -1 1 0 3]);

Arm_Parameters;
M1=Arm_Plot3D(0,0,0,0,0,q1(1),q2(1),q3(1),0);

for i=1:pasos:length(t)

delete (M1)
M1=Arm_Plot3D(0,0,0,0,0,q1(i),q2(i),q3(i),0),hold on;
plot3(xr(1:i),yr(1:i),zr(1:i),'r','linewidth',3);
pause(0.1)

end