plotquadrotor.m

四轴飞行器Matlab仿真分析,涉及四轴飞行器主要部件如Motor,camera,电池等

% translate vertices by column vector T
function [X,Y,Z] = translateXYZ(X,Y,Z,T)

  X = X + T(1)*ones(size(X));
  Y = Y + T(2)*ones(size(Y));
  Z = Z + T(3)*ones(size(Z));

% end translateXYZ

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_quad, Face_quad, colors_quad] = quadrotorVertFace(P)
% Vertices = [x,y,z] position of each vertex
% Faces = defines how the vertices are connnected for form faces.  Each set
% of our vertices defines one face.


%--------- vertices and faces for center pod ------------
% vertices of the center pod
Vert_center = [...
    P.w/2, P.w/2, P.w/2;...
    P.w/2, P.w/2, -P.w/2;...
    P.w/2, -P.w/2, -P.w/2;...
    P.w/2, -P.w/2, P.w/2;...
    -P.w/2, P.w/2, P.w/2;...
    -P.w/2, P.w/2, -P.w/2;...
    -P.w/2, -P.w/2, -P.w/2;...
    -P.w/2, -P.w/2, P.w/2;...
    ];
% define faces of center pod
Face_center = [...
        1, 2, 3, 4;... % front
        5, 6, 7, 8;... % back
        1, 4, 8, 5;... % top
        8, 4, 3, 7;... % right 
        1, 2, 6, 5;... % left
        2, 3, 7, 6;... % bottom
        ];
    
%--------- vertices and faces for connecting rods ------------    
% vertices for front rod
Vert_rod_front = [...
    P.w/2, P.lw/2, 0;...
    P.w/2, -P.lw/2, 0;...
    P.w/2, 0, P.lw/2;...
    P.w/2, 0, -P.lw/2;...
    P.l+P.w/2, P.lw/2, 0;...
    P.l+P.w/2, -P.lw/2, 0;...
    P.l+P.w/2, 0, P.lw/2;...
    P.l+P.w/2, 0, -P.lw/2;...
    ];
Face_rod_front = 8 + [...
        1, 2, 6, 5;... % x-y face
        3, 4, 8, 7;... % x-z face
    ];
% vertices for right rod
Vert_rod_right = Vert_rod_front*[0,1,0;1,0,0;0,0,1];
Face_rod_right = 16 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];
% vertices for back rod
Vert_rod_back = Vert_rod_front*[-1,0,0;0,-1,0;0,0,1];
Face_rod_back = 24 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];
% vertices for left rod
Vert_rod_left = Vert_rod_front*[0,-1,0;-1,0,0;0,0,1];
Face_rod_left = 32 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];

%--------- vertices and faces for rotors ------------  
Vert_rotor = [];
for i=1:P.N,
    Vert_rotor = [Vert_rotor; P.r*cos(2*pi*i/P.N), P.r*sin(2*pi*i/P.N), 0];
end
for i=1:P.N,
    Vert_rotor = [Vert_rotor; P.r/10*cos(2*pi*i/P.N), P.r/10*sin(2*pi*i/P.N), 0];
end
Face_rotor = [];
for i=1:P.N-1,
    Face_rotor = [Face_rotor; i, i+1, P.N+i+1, P.N+i];
end
Face_rotor = [Face_rotor; P.N, 1, P.N+1, 2*P.N];

% front rotor
Vert_rotor_front = Vert_rotor + repmat([P.w/2+P.l+P.r, 0, 0],2*P.N,1);
Face_rotor_front = 40 + Face_rotor;
% right rotor
Vert_rotor_right = Vert_rotor + repmat([0, P.w/2+P.l+P.r, 0],2*P.N,1);
Face_rotor_right = 40 + 2*P.N + Face_rotor;
% back rotor
Vert_rotor_back = Vert_rotor + repmat([-(P.w/2+P.l+P.r), 0, 0],2*P.N,1);
Face_rotor_back = 40 + 2*P.N + 2*P.N + Face_rotor;
% left rotor
Vert_rotor_left = Vert_rotor + repmat([0, -(P.w/2+P.l+P.r), 0],2*P.N,1);
Face_rotor_left = 40 + 2*P.N + 2*P.N + 2*P.N + Face_rotor;

% collect all of the vertices for the quadrotor into one matrix
Vert_quad = [...
    Vert_center; Vert_rod_front; Vert_rod_right; Vert_rod_back;...
    Vert_rod_left; Vert_rotor_front; Vert_rotor_right;...
    Vert_rotor_back; Vert_rotor_left...
    ];
% collect all of the faces for the quadrotor into one matrix
Face_quad = [...
    Face_center; Face_rod_front; Face_rod_right; Face_rod_back;...
    Face_rod_left; Face_rotor_front; Face_rotor_right;...
    Face_rotor_back; Face_rotor_left...
    ];

myred = [1, 0, 0];
mygreen = [0, 1, 0];
myblue = [0, 0, 1];
myyellow = [1,1,0];

colors_quad = [...
    mygreen;... % fuselage front
    myblue;... % back
    myyellow;... % top
    myblue;... % right
    myblue;... % left
    myblue;... % bottom
    mygreen;... % rod front
    mygreen;... %
    mygreen;... % rod right
    mygreen;... %
    mygreen;... % rod back
    mygreen;... %
    mygreen;... % rod left
    mygreen;... %
    ];
for i=1:P.N,
    colors_quad = [colors_quad; mygreen];   % front rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % right rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % left rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % back rotor
end

% end quadrotorVertFace

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_rails, Face_rails, colors_rails] = railVertFace(P)
% Vertices = [x,y,z] position of each vertex
% Faces = defines how the vertices are connnected for form faces.  Each set
% of our vertices defines one face.

%-------vertices and faces for rails--------------
% vertices for right rail
Vert_rail_right = [...
    P.w/2+P.l, P.l_rail/2, P.w_rail;...
    P.w/2+P.l, P.l_rail/2, 0;...
    P.w/2+P.l, -P.l_rail/2, 0;...
    P.w/2+P.l, -P.l_rail/2, P.w_rail;...
    P.w/2+P.l+P.w_rail, P.l_rail/2, 0;...
    P.w/2+P.l+P.w_rail, -P.l_rail/2, 0;...
    ];
Face_rail_right = [...
        1, 2, 3, 4;... % x-y face
        5, 2, 3, 6;... % x-z face
    ];

% vertices for left rail
Vert_rail_left = Vert_rail_right*[-1,0,0;0,1,0;0,0,1];
Face_rail_left = 6 + [...
        1, 2, 3, 4;... % x-y face
        5, 2, 3, 6;... % x-z face
    ];

% collect vertices and faces
Vert_rails = [Vert_rail_right; Vert_rail_left];
Face_rails = [Face_rail_right; Face_rail_left];

colors_rails = [...
    P.myyellow;...% right rail
    P.myyellow;...%
    P.myyellow;...% left rail
    P.myyellow;...%
    ];

% end railVertFace

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_fov, Face_fov, colors_fov] = fovVertFace(pn,pe,pd,phi,theta,psi,P,C)

%-------vertices and faces for camera field-of-view --------------
% vertices 
Vert_fov = [...
    pn, pe, -pd;...
    pn+pd*tan(phi+C.cam_fov/2), pe+pd*tan(-theta+C.cam_fov/2), 0;...
    pn+pd*tan(phi+C.cam_fov/2), pe+pd*tan(-theta-C.cam_fov/2), 0;...
    pn-pd*tan(-phi+C.cam_fov/2), pe+pd*tan(-theta+C.cam_fov/2), 0;...
    pn-pd*tan(-phi+C.cam_fov/2), pe+pd*tan(-theta-C.cam_fov/2), 0;...
    ];
Vert_fov = rotateVert(Vert_fov,0,0,-psi);
Face_fov = [...
        1, 1, 2, 2;... % x-y face
        1, 1, 3, 3;... % x-y face
        1, 1, 4, 4;... % x-y face
        1, 1, 5, 5;... % x-y face
        2, 3, 5, 4;... % x-y face
    ];

colors_fov = [P.myblue; P.myblue; P.myblue; P.myblue; P.myyellow];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X,Y,Z] = targetXYZ
  
  pts = [...
      2, 0, 0;...
      0, 2, 0;...
      0, 1, 0;...
      -2, 1, 0;...
      -2, -1, 0;...
      0, -1, 0;...
      0, -2, 0;...
      2, 0, 0;...
      ];
  X = pts(:,1);
  Y = pts(:,2);
  Z = pts(:,3);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle = targetPlot(tn,te,td,tpsi,size,handle,color,mode)

  % get the target data points
  [X,Y,Z] = targetXYZ;
  
  % scale by size
  X = X*size;
  Y = Y*size;
  Z = Z*size;
  
  % rotate by tpsi
  [X,Y,Z] = rotateXYZ(X,Y,Z,0,0,tpsi);
  
  % translate by [tn, te, td]
  [X,Y,Z] = translateXYZ(X,Y,Z,[tn,te,td]);
  
  if isempty(handle),
%    handle = patch(X, Y, Z, color, 'EraseMode', mode);
    handle = patch(Y, X, -Z, color, 'EraseMode', mode);
  else
%    set(handle,'XData',X,'YData',Y,'ZData',Z);
    set(handle,'XData',Y,'YData',X,'ZData',-Z);
    drawnow
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle = targetCamPlot(tn,te,td,tpsi,size,handle,color,mode)

  % get the target data points
  [X,Y,Z] = targetXYZ;
  
  % scale by size
  X = X*size;
  Y = Y*size;
  Z = Z*size;
  
  % rotate by tpsi
  [X,Y,Z] = rotateXYZ(X,Y,Z,0,0,tpsi);
  
  % translate by [tn, te, td]
  [X,Y,Z] = translateXYZ(X,Y,Z,[tn,te,td]);
  
  if isempty(handle),
    handle = patch(X, Y, Z, color, 'EraseMode', mode);
  else
    set(handle,'XData',X,'YData',Y,'ZData',Z);
    drawnow
  end