%机械手臂的控制PD+PD前馈控制+PD修正前馈控制
function a=PD_control_feedforward()
clc
clear all
close all
global F
global Select
global kp1 kv1 kp2 kv2
%%%%%%%%%%%%%%%%%%%%initial vallue and set the parameter of system %%%%%%%%
F=2;                %tack the two locus
Select=3;           %select the thrid method as control law
%------------------------PD参数的调整是核心--------------------------%
kp1=3000;kv1=230;   %PD parameters
 kp2=2000;kv2=210;
%-------------------------------------------------------------%
ts=0.001; Tmax=5;Timeset=[0:ts:Tmax];     %仿真时间和步长
X0=[0.5,2,3,4];                             %初始状态
para=[];
[t,y]=ode45(@fun,Timeset,X0,[],para); %'fun' 代表文件名。@fun代表函数名
switch F
    case 1
        r1=pi/6;r2=pi/6;
        dr1=0;dr2=0;
        ddr1=0;ddr2=0;
    case 2
        r1=sin(3*t);r2=cos(3*t);
        dr1=3*cos(3*t);dr2=-3*sin(3*t);
        ddr1=-3*3*sin(3*t);ddr2=-3*3*cos(3*t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%结果输出%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
plot(t,r1,'b',t,y(:,1),'r');grid on       %关节角1
legend('angle_one');
%figure(2)
%plot(t,dr1,'b',t,y(:,2),'r');grid on
%legend('dangle one');
figure(3)
plot(t,r2,'b',t,y(:,3),'r');grid on       %关节角2
legend('angle two');
%figure(4)
%plot(t,dr2,'b',t,y(:,4),'r');grid on
%legend('dangle two');      
end

function dx=fun(t,x,flag,para)
global F
global Select
global kp1 kv1 kp2 kv2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%system model%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %be control model 模型准备  M*ddq+C*dq+G=Tao
x1=x(1);x3=x(3);x2=x(2);x4=x(4);
 D11=2.462;D22=0.362;D12=0.147;D21=0.147;
 M=[D11 D12*cos(x1-x3);D21*cos(x1-x3),D22];
 C=[-x4*sin(x3),-(x2+x4)*sin(x3);x2*sin(x3),0];
 G=[D12*x4^2*sin(x1-x3);-D12*x2^2*sin(x1-x3)];
%tracking locus
switch F
    case 1
        r1=pi/6;r2=pi/6;
        dr1=0;dr2=0;
        ddr1=0;ddr2=0;
    case 2
        r1=sin(3*t);r2=cos(3*t);
        dr1=3*cos(3*t);dr2=-3*sin(3*t);
        ddr1=-3*3*sin(3*t);ddr2=-3*3*cos(3*t);
end
%性能指标
 dr=[dr1;dr2];
 ddr=[ddr1;ddr2];
 e1=x(1)-r1;de1=x(2)-dr1;
 e2=x(3)-r2;de2=x(4)-dr2;
 %%%%%%%%%%%%%%%%%%%%%%%%计算输出力矩%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 switch Select
     case 1                      %PD控制
         u1=-kp1*e1-kv1*de1;
         u2=-kp2*e2-kv2*de2;
     case 2                      %PD+前馈控制
         u1=-kp1*e1-kv1*de1+M(1,:)*ddr+C(1,:)*dr+G(1);
         u2=-kp2*e2-kv2*de2+M(2,:)*ddr+C(2,:)*dr+G(2);
     case 3                      %PD控制+修正的前馈控制
       x1=r1;x2=dr1;x3=r2;x4=dr2; 
    Md=[D11 D12*cos(x1-x3);D21*cos(x1-x3),D22];
   Cd=[-x4*sin(x3),-(x2+x4)*sin(x3);x2*sin(x3),0];
   Gd=[D12*x4^2*sin(x1-x3);-D12*x2^2*sin(x1-x3)];
       u1=-kp1*e1-kv1*de1+Md(1,:)*ddr+Cd(1,:)*dr+Gd(1);
         u2=-kp2*e2-kv2*de2+Md(2,:)*ddr+Cd(2,:)*dr+Gd(2);
 end
  tol=[u1;u2];                        %每步仿真的计算控制力矩输出
 %%%%%%%%%%%%%%%%%%%%%求解加速度导数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  Q=inv(M)*(tol-C*[x(2);x(4)])-G;      %根据动力学模型建立的方程
dx(1)=x(2);                            %一阶微分方程组结果
dx(2)=Q(1);
dx(3)=x(4);
dx(4)=Q(2);
dx=dx.';                                 %结果输出必须为列向量
end