-
Notifications
You must be signed in to change notification settings - Fork 0
/
drone_Animation.m
130 lines (118 loc) · 5.34 KB
/
drone_Animation.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
function drone_Animation(x,y,z,roll,pitch,yaw,aniON)
% This Animation code was written for QuadCopter by Jitendra Singh.
% And edited and revised by Dogan Yildiz for octocopter animation.
% Copyright 2019 The MathWorks, Inc
%% Define design parameters
D2R = pi/180;
R2D = 180/pi;
b = 1.0; % the length of total square cover by whole body of quadcopter in meter
a = b/3; % the legth of small square base of quadcopter(b/4)
H = 0.06; % hight of drone in Z direction (4cm)
H_m = H+H/2; % hight of motor in z direction (5 cm)
r_p = b/6; % radius of propeller
%% Conversions
ro = 45*D2R; % angle by which rotate the base of quadcopter
Ri = [cos(ro) -sin(ro) 0;
sin(ro) cos(ro) 0;
0 0 1]; % rotation matrix to rotate the coordinates of base
base_co = [-a/2 a/2 a/2 -a/2; % Coordinates of Base
-a/2 -a/2 a/2 a/2;
0 0 0 0];
base = Ri*base_co; % rotate base Coordinates by 45 degree
to = linspace(0, 2*pi);
xp = r_p*cos(to);
yp = r_p*sin(to);
zp = zeros(1,length(to));
%% Define Figure plot
fig1 = figure('pos', [0 50 800 600]);
hg = gca;
view([138 20])
grid on;
axis equal;
zlim([9 12]);
title('Drone Animation')
xlabel('X[m]');
ylabel('Y[m]');
zlabel('Z[m]');
hold(gca, 'on');
%% Design Different parts
% design the base square
drone(1) = patch([base(1,:)],[base(2,:)],[base(3,:)],'r');
drone(2) = patch([base(1,:)],[base(2,:)],[base(3,:)+H],'r');
alpha(drone(1:2),0.7);
% design 2 parpendiculer legs of quadcopter
[xcylinder ycylinder zcylinder] = cylinder([H/2 H/2]);
drone(3) = surface(b*zcylinder-b/2,ycylinder,xcylinder+H/2,'facecolor','g');
drone(4) = surface(ycylinder,b*zcylinder-b/2,xcylinder+H/2,'facecolor','b');
zcylinderRot = b*zcylinder-b/2;
ycylinderRot = ycylinder;
xcylinderRot = xcylinder+H/2;
transformedCylinder1= Ri*[zcylinderRot(1,:);ycylinderRot(1,:);xcylinderRot(1,:)];
transformedCylinder2= Ri*[zcylinderRot(2,:);ycylinderRot(2,:);xcylinderRot(2,:)];
drone(5) = surface([transformedCylinder1(1,:);transformedCylinder2(1,:)],[transformedCylinder1(2,:);transformedCylinder2(2,:)],[transformedCylinder1(3,:);transformedCylinder2(3,:)],'facecolor','b');
ro = -45*D2R;
Ri = [cos(ro) -sin(ro) 0;
sin(ro) cos(ro) 0;
0 0 1];
zcylinderRot = b*zcylinder-b/2;
ycylinderRot = ycylinder;
xcylinderRot = xcylinder+H/2;
transformedCylinder1= Ri*[zcylinderRot(1,:);ycylinderRot(1,:);xcylinderRot(1,:)];
transformedCylinder2= Ri*[zcylinderRot(2,:);ycylinderRot(2,:);xcylinderRot(2,:)];
drone(6) = surface([transformedCylinder1(1,:);transformedCylinder2(1,:)],[transformedCylinder1(2,:);transformedCylinder2(2,:)],[transformedCylinder1(3,:);transformedCylinder2(3,:)],'facecolor','b');
alpha(drone(3:6),0.6);
% design 4 cylindrical motors
drone(7) = surface(xcylinder+b/2,ycylinder,H_m*zcylinder+H/2,'facecolor','r');
drone(8) = surface(xcylinder-b/2,ycylinder,H_m*zcylinder+H/2,'facecolor','r');
drone(9) = surface(xcylinder,ycylinder+b/2,H_m*zcylinder+H/2,'facecolor','r');
drone(10) = surface(xcylinder,ycylinder-b/2,H_m*zcylinder+H/2,'facecolor','r');
drone(11) = surface(xcylinder+b/2*sqrt(2)/2,ycylinder+b/2*sqrt(2)/2,H_m*zcylinder+H/2,'facecolor','r');
drone(12) = surface(xcylinder+b/2*sqrt(2)/2,ycylinder-b/2*sqrt(2)/2,H_m*zcylinder+H/2,'facecolor','r');
drone(13) = surface(xcylinder-b/2*sqrt(2)/2,ycylinder+b/2*sqrt(2)/2,H_m*zcylinder+H/2,'facecolor','r');
drone(14) = surface(xcylinder-b/2*sqrt(2)/2,ycylinder-b/2*sqrt(2)/2,H_m*zcylinder+H/2,'facecolor','r');
alpha(drone(7:14),0.7);
% design 4 propellers
drone(15) = patch(xp+b/2,yp,zp+(H_m+H/2),'r','LineWidth',0.5);
drone(16) = patch(xp-b/2,yp,zp+(H_m+H/2),'c','LineWidth',0.5);
drone(17) = patch(xp,yp+b/2,zp+(H_m+H/2),'c','LineWidth',0.5);
drone(18) = patch(xp,yp-b/2,zp+(H_m+H/2),'b','LineWidth',0.5);
drone(19) = patch(xp+b/2*sqrt(2)/2,yp+b/2*sqrt(2)/2,zp+(H_m+H/2),'c','LineWidth',0.5);
drone(20) = patch(xp+b/2*sqrt(2)/2,yp-b/2*sqrt(2)/2,zp+(H_m+H/2),'c','LineWidth',0.5);
drone(21) = patch(xp-b/2*sqrt(2)/2,yp+b/2*sqrt(2)/2,zp+(H_m+H/2),'c','LineWidth',0.5);
drone(22) = patch(xp-b/2*sqrt(2)/2,yp-b/2*sqrt(2)/2,zp+(H_m+H/2),'c','LineWidth',0.5);
alpha(drone(15:22),0.3);
%% create a group object and parent surface
combinedobject = hgtransform('parent',hg );
set(drone,'parent',combinedobject)
if(aniON == 1)
filename = 'octoAnimation.gif';
frameCount = 1;
end
for i = 1:5:length(x)
ba = plot3(x(1:i),y(1:i),z(1:i), 'b:','LineWidth',1.5);
% Uncomment below line if moving with frame is desired.
% xlim([min([x(i)-1 x(i)+1]) max([x(i)-1 x(i)+1])]); ylim([min([y(i)-1 y(i)+1]) max([y(i)-1 y(i)+1])]);
translation = makehgtform('translate',...
[x(i) y(i) z(i)]);
rotation1 = makehgtform('xrotate',(pi/180)*(roll(i)));
rotation2 = makehgtform('yrotate',(pi/180)*(pitch(i)));
rotation3 = makehgtform('zrotate',(pi/180)*yaw(i));
set(combinedobject,'matrix',...
translation*rotation3*rotation2*rotation1);
drawnow
if(aniON == 1)
frameF(frameCount) = getframe(fig1);
frameCount = frameCount + 1;
end
end
if(aniON == 1)
writerObj = VideoWriter(filename);
writerObj.FrameRate = 10;
open(writerObj);
for i=1:length(frameF)
frame = frameF(i) ;
writeVideo(writerObj, frame);
end
close(writerObj);
end
end