%=====================================================================

=====
%
MO PHONG ROBOT THETA-R-de 12
%=====================================================================
=====
%----------------------------------------------------------%
%---bo dieu khien PD kinh dien
----------------%
%---tham so KP,KD chon truoc
----------------%
%---qui dao thiet ke 2-1-2
----------------%
%----------------------------------------------------------%
%=====================================================================
=====
%Tham so robot
m1=1;m2=1; %khoi luong dat tai cuoi cac khop.
theta_0=0;
theta_max=pi/3;
r1=0.6;r_max=1.2% Vi tri dau va cuoi hanh trinh chuyen dong robot
k=0;
%Tham so thoi gian cho tinh qui dao khop
tc=6;
%thoi gian thuc hien hanh trinh chuyen dong
t1=1;
%ket thuc qua trinh khoi dong
t2=5;
%bat dau qua trinh ham
Tk=0.001;
%thoi gian trich mau
%------------------------------------------------------------------------%Vi tri khop dau va cuoi
q0=[theta_0;r1];
qc=[theta_max;r_max];
%gia toc cac khop tinh toan duoc
ddq1=pi/15 ; %rad/s^2
ddq2=.1 ;
%m/s^2
ddq=[ddq1;ddq2];
%Dieu kien dau
q=q0;
dq=[0;0];
%------------------------------------------------------------------------for t=0:0.001:tc;
k=k+1;
%--------------------------------------------------------------------%Tinh toan vi tri va toc do dat tu dang qui dao da dinh truoc
if t<=t1
qd=q0+ddq*t^2/2;
dqd=ddq*t;
elseif(t>t1)&(t<=t2)
qd=q0+ddq*t1*(t-t1/2);
dqd=ddq*t1;
elseif(t>t2)&(t<tc)

qd=qc-ddq*(t-tc)^2/2;
dqd=ddq*t1-ddq*(t-t2);
end
%--------------------------------------------------------------------%Tinh momen dieu khien tu phuong trinh momen dieu khien
kp1=200;
kp2=200;
kd1=5;
kd2=5;
Kp=[kp1,0;0,kp2];
%Ma tran he so KP
Kd=[kd1,0;0,kd2];
%Ma tran he so KD
error=qd-q;
%Sai lech vi tri
derror=dqd-dq;
%Sai lech toc do
M=Kp*error+Kd*derror; %Momen dieu khien
%--------------------------------------------------------------------%Tinh vi tri thuc va toc do thuc tu mo hinh dong luc hoc he thong
%Ma tran quan tinh
H11=m1*q0(2)^2+m2*q(2)^2;
H12=0;
H21=0;
H22=m2;
H=[H11 H12;H21 H22];
%Ma tran quan tinh nghich dao
Hinv=inv(H);
%Momen nhot va quan tinh
V1=2*m2*q(2)*dq(2)*dq(1);
V2=-m2*q(2)*dq(1)^2;
V=[V1;V2];
%Trong luong
G1=(m1*q0(2)+m2*q(2))*9.81*cos(q(1));
G2=m2*9.81*sin(q(1));
G=[G1;G2];
%Gia toc
gia_toc=-Hinv*(V)+Hinv*M;

%Khong co thanh phan G vi tu gia thiet

coi
%nhu trong truong da duoc bu
%Tuyen tinh hoa quanh diem lam viec
dq=dq+gia_toc*Tk;
q=q+dq*Tk;
%--------------------------------------------------------------------%Luu cac bien can ve vao mang
x(k)=t;
%Mang luu cac thoi diem trich mau
%Khop quay theta
y_qd1(k)=qd(1);
y_q1(k)=q(1);
y_dqd1(k)=dqd(1);

%Mang luu vi tri khop quay Theta dat truoc

%Mang luu vi tri khop quay Theta thuc
%Mang luu toc do khop quay Theta dat truoc

y_dq1(k)=dq(1);
%Mang luu toc do khop quay Theta thuc
y_ddq1(k)=gia_toc(1);
%Mang luu gia toc khop quay Theta
y_er1(k)=error(1);
%Mang luu sai lech vi tri khop quay Theta
y_der1(k)=derror(1);
%Mang luu sai lech toc do khop quay Theta
y_momen(k)=M(1);
%Mang luu Momen dieu khien khop quay Theta
%Khop tinh tien
y_qd2(k)=qd(2);
%Mang luu vi tri khop tinh tien dat truoc
y_q2(k)=q(2);
%Mang luu vi tri khop tinh tien thuc
y_dqd2(k)=dqd(2);
%Mang luu toc do khop tinh tien dat truoc
y_dq2(k)=dq(2);
%Mang luu toc do khop tinh tien thuc
y_ddq2(k)=gia_toc(2);
%Mang luu gia toc khop tinh tien
y_er2(k)=error(2);
%Mang luu sai lech vi tri khop tinh tien
y_der2(k)=derror(2);
%Mang luu sai lech toc do khop tinh tien
y_Luc(k)=M(2);
%Mang luu Momen dieu khien khop tinh tien
end
% Ve cho khop quay theta
figure(1);
subplot(221);
plot(x,y_qd1);
hold on
plot(x,y_q1,'r');
%plot(x,y_q1,'k-.');
xlabel('time');
ylabel('rad');
title('Vi tri khop 1');
subplot(222);
plot(x,y_dqd1);
hold on
plot(x,y_dq1,'r');
%plot(x,y_dq1,'k-.');
xlabel('time');
ylabel('rad/s');
title('Toc do khop 1');
subplot(223);
plot(x,y_ddq1,'r');
%plot(x,y_ddq1,'k-.');
hold on
xlabel('time');
ylabel('rad/s2');
title('gia toc khop 1');
subplot(224);
plot(x,y_momen,'r');
%plot(x,y_momen,'k-.');
hold on
xlabel('time');
ylabel('N.m');
title('Momen khop 1');

% Ve cho khop tinh tien

figure(2);
subplot(221);
plot(x,y_qd2);
hold on
plot(x,y_q2,'r');
%plot(x,y_q2,'k-.');
xlabel('time');
ylabel('rad');
title('Vi tri khop 2');
subplot(222);
plot(x,y_dqd2);
hold on
plot(x,y_dq2,'r');
%plot(x,y_dq2,'k-.');
xlabel('time');
ylabel('rad/s');
title('Toc do khop 2');
subplot(223);
plot(x,y_ddq2,'r');
%plot(x,y_ddq2,'k-.');
hold on
xlabel('time');
ylabel('rad/s2');
title('gia toc khop 2');
subplot(224);
plot(x,y_Luc,'r');
%plot(x,y_Luc,'k-.');
hold on
xlabel('time');
ylabel('N.m');
title('Momen khop 2');