PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

EJERCICIO 2.1
% Simula el movimiento circular de un robot R
% Lo representa en la referencia absoluta W
% Definir xWR (localizacin del robot R en W)
% Definir el radio de curvatura R
% definir o calcular w (velocidad angular)
xWR=[0 0 pi()/2]; %definimos la posicin del robot a 90 respecto de la absoluta

R=0;
w=-0.4;
Rrueda=0.028;
L=0.11;
paso=1;
velo=[]; %vector velocidad lineal del robot
angulares=[];%vector velocidad angular del robot
angularesi=[]; %vector velocidad angular de la rueda izquierda
angularesd=[]; %vector velocidad angular de la rueda derecha
final=0;
pk=[0 0];
titak=0;
Lk=[pk titak];
t=0;
tfinal=10;
% Tiempo final de simulacin
T=0.05;
% Periodo de muestreo de simulacin
figure(1)
dibrobot(xWR); % Dibuja el robot en la referencia absoluta
hold on
while (final==0),
t=t+T;
dtitak=w*T;
titak=norm_pi(titak+dtitak);
% Normaliza el ngulo entre [-pi, pi]
pk=[R*sin(titak) R*(1-cos(titak))]; %Actualiza la pos en mov circurlar
xRk=[pk titak]; %Actualiza la pos y ori en mov circurlar
wd=v/Rrueda+w*L/(2*Rrueda); %velocidad angular de la rueda dch en
vehiculo diferencial
wi=v/Rrueda-w*L/(2*Rrueda); %velocidad angular de la rueda izq en
vehiculo diferencial
Lk=[Lk;xRk];
velo=[velo v];
angulares=[angulares w];
angularesi=[angularesi wi];
angularesd=[angularesd wd];
locrob=loc(hom(xWR)*hom(xRk)); % Localizacin en la referencia absoluta
dibrobot(locrob);
pause(0.01);

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

%Fin Reorientado
if (titak<-pi()/2 && paso==1)
xWR=locrob;
titak=0;
v=1;
R=1;
w=v/R;
paso=2;
end
%Fin Tramo1
if (titak<0 && paso==2)
xWR=locrob;
titak=0;
R=-R;
w=v/R;
paso=3;
end
if (titak>0 && paso==3)
paso=4;
end
% Fin Tramo 2
if (titak<0 && paso==4)
R=-R;
w=v/R;
xWR=locrob;
titak=0;
paso=5;
end
% Fin Tramo 3
if (titak<0 && paso==5)
final=1;
end
end
hold off;

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

% Graficas
tiempos=[T:T:t];
figure(2)
plot(tiempos, velo);
xlabel('numero de periodo');
ylabel('velocidad lineal (m/s)')

Durante los 4 primeros periodos, correspondientes a la reorientacin, la

velocidad lineal es 0 y a partir de ese momento se mantiene constante.
figure(3)
plot(tiempos, angulares)
xlabel('numero de periodo');
ylabel('velocidad angular (rad/s)')

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

Durante la reorientacin la velocidad angular se mantiene

constante. En los pasos, dependiendo del giro tiene signo
negativo(a derechas) o positivo(a izquierdas).
plot(tiempos, angularesi)
xlabel('numero de periodo');
ylabel('velocidad angular de la rueda izq (rad/s)')

figure(5)
plot(tiempos, angularesd)
xlabel('numero de periodo');
ylabel('velocidad angular de la rueda drcha (rad/s)')

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

%Los cambios de trayectoria se detectan observando el angulo del robot

%El tiempo total del recorrido es 25 seg.

EJERCICIO 2.2
% Simula el movimiento circular de un robot R
% Lo representa en la referencia absoluta W
% Definir xWR (localizacin del robot R en W)
% Definir el radio de curvatura R
% definir o calcular w (velocidad angular)
r1=3;
%definimos el radio de la primera circunferencia
r2=2; %definimos el radio de la segunda circunferencia
xWR=[0 0 0];
v=1;
Rrueda=0.1;
L=0.5;
R=0;
w=0;
paso=1;
velo=[];
angulares=[];
angularesi=[];
angularesd=[];
locabs=[];
tactual=0;
final=0;
pk=[0 0];
titak=0;
Lk=[pk titak];
t=0;
tfinal=10;
% Tiempo final de simulacin
T=0.05;
% Periodo de muestreo de simulacin
figure(1)
dibrobot(xWR); % Dibuja el robot en la referencia absoluta
hold on
while (final==0),
t=t+T;
dtitak=w*T;
titak=norm_pi(titak+dtitak);

% Normaliza el ngulo entre [-pi, pi]

if (w==0) % Calculo de la posicin para tramo recto

pk=[v*(t-tactual) 0];
xRk=[pk 0];
end
if (w~=0) % Calculo de la posicin para tramo curvo

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

pk=[R*sin(titak) R*(1-cos(titak))];
xRk=[pk titak];
end
wd=v/Rrueda+w*L/(2*Rrueda); %velocidad angular de la rueda dch en
vehiculo diferencial
wi=v/Rrueda-w*L/(2*Rrueda); %velocidad angular de la rueda izq en
vehiculo diferencial

Lk=[Lk;xRk];
velo=[velo v];
angulares=[angulares w];
angularesi=[angularesi wi];
angularesd=[angularesd wd];
locrob=loc(hom(xWR)*hom(xRk)); % Localizacin en la referencia absoluta
locabs=[locabs;locrob];
dibrobot(locrob);
pause(0.01);
%TRAMO 2, CIRCUNFERENCIA 1
if (t>8 && paso==1)
%ya que en 8 seg, a 1m/s habr recorrido los 8 m
xWR=locrob;
titak=0;
R=-r1;
w=v/R;
paso=2;
end
if (titak>0 && paso==2)
paso=3;
end
%TRAMO 3, RECTO
if (titak<2.89 && paso==3)
xWR=locrob;
w=0;
paso=4;
tactual=t;
end
%TRAMO 4, CIRCUNFERENCIA 2
if ((t-tactual)>8 && paso==4)
xWR=locrob;
titak=0;
R=-r2;
w=v/R;
paso=5;
end

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

%FIN
if (titak<-2.89 && paso==5)
final=1;
end
end
hold off;

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

%Grficas
tiempos=[T:T:t];
figure(2)
plot(tiempos, velo)
xlabel('numero de periodo');
ylabel('velocidad lineal (m/s)')

Velocidad lineal constante en todos los tramos.

figure(3)
plot(tiempos, angulares)
xlabel('numero de periodo');
ylabel('velocidad angular (rad/s)')

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

En los pasos, dependiendo del giro, la velocidad angular tiene

signo negativo (a derechas) o positivo(a izquierdas).
figure(4)
plot(tiempos, angularesi)
xlabel('numero de periodo');
ylabel('velocidad angular de la rueda izq (rad/s)')

En los tramos rectos la velocidad angular de la rueda izq es 10. En los tramos
curvos la velocidad angular pasa a tomar un valor mayor cuanto mayor es el
radio.
figure(5)
plot(tiempos, angularesd)
xlabel('numero de periodo');
ylabel('velocidad angular de la rueda drcha (rad/s)')

PASCUAL NAVARRO, Begoa

PASCUAL NAVARRO, Cristina

En los tramos rectos la velocidad angular de la rueda drch es 10. En los tramos
curvos la velocidad angular pasa a tomar un valor mayor cuanto mayor es el
radio.
Si comparamos estas dos ltimas grficas podemos ver como en ambos giros el
valor de velocidad de la rueda izq es mayor que el de la derecha puesto que
el giro es a derechas.

%Los cambios de trayectoria se detectan observando el angulo del robot

%El tiempo total del recorrido es 32 seg.