-
Notifications
You must be signed in to change notification settings - Fork 1
/
draw_results_pitch.m
159 lines (126 loc) · 5.29 KB
/
draw_results_pitch.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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
%---------------------------------------------------------
%
% This function draws the OS4 robot.
%
%
% Developed by Marcelo Becker & Samir Bouabdallah
% EPFL - October, 2005
%
% BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
% FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
% OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
% PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
% OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
% TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
% PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
% REPAIR OR CORRECTION.
%
% 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
% WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
% REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
% INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
% OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
% TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
% YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
% PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGES.
%----------------------------------------------------------
function draw_results_pitch(theta)
% Global variables declaration
global SwitchProc1 % used for the Selection of the Procedures
global GRAF % Radio Button
global EDIT % Edit Text on Robox Window
global EIXOS EIXOS1 EIXOS2 EIXOS3 EIXOS4 EIXOS5 EIXOS6
global FIGU FIGU1 FIGU2 FIGU3 FIGU4 % Figure parameters
global LINHA XLIMI YLIMI
global x y z
global roll pitch yaw tout
% -------------------------------------------------------------------------
R1 = 0.15; % OS4 Rotor radius [m]
R2 = 0.37; % OS4 base [m]
R3 = 0.01;
R4 = 0.2;
L1 = 0.1; % OS4 Avionics Box length [m]
H1 = 0.1; % OS4 Avionics Box height [m]
ang1= 5*pi/180; % OS4 motor structure inclination angle [rd]
cth=cos(theta);
sth=sin(theta);
% XZ PLANE
% --------
% Local Frame
xxc = [0; cth*R4];
zzc = [0; -sth*R4];
LINHA=line(xxc, zzc, 'Parent',EIXOS2,'Color',[1 0 0],'LineStyle','-', 'LineWidth',2);
pz=[-0.02 0 0.02 0 -0.02];
px=[(R4-0.02) (R4+0.02) (R4-0.02) R4 (R4-0.02)];
npx = cth*px + sth*pz;
npz = cth*pz - sth*px;
efe=patch(npx, npz, [1 0 0],'LineStyle','none','Parent',EIXOS2);
xxc = [0; sth*R4];
zzc = [0; cth*R4];
LINHA=line(xxc, zzc, 'Parent',EIXOS2,'Color',[0 1 1],'LineStyle','-', 'LineWidth',2);
px=[-0.02 0 0.02 0 -0.02];
pz=[(R4-0.02) (R4+0.02) (R4-0.02) R4 (R4-0.02)];
npx = cth*px + sth*pz;
npz = cth*pz - sth*px;
efe=patch(npx, npz, [0 1 1],'LineStyle','none','Parent',EIXOS2);
% OS4 Avionics Box
gh = L1*sqrt(2)/2;
px = [-gh gh gh -gh -gh];
pz = [L1/2 L1/2 -L1/2 -L1/2 L1/2];
npz = cth*pz - sth*px;
npx = sth*pz + cth*px;
efe=patch(npx, npz,[0.5 0.7 0.5],'Parent',EIXOS2);
npz = cth*L1/2;
npx = sth*L1/2;
xxc = [0; npx];
zzc = [0; npz];
LINHA=line(xxc, zzc,'Parent',EIXOS2,'Color',[0 0 0],'LineStyle','-', 'LineWidth',1);
npz = -cth*L1/2;
npx = -sth*L1/2;
xxc = [0; npx];
zzc = [0; npz];
LINHA=line(xxc, zzc,'Parent',EIXOS2,'Color',[0 0 0],'LineStyle','-', 'LineWidth',1);
% OS4 Structure bars and Rotors
hlinha = R2*tan(ang1);
h2inha = 0.06 - hlinha;
npz = -sth*R2 + cth*(hlinha + h2inha + L1/2);
npx = cth*R2 + sth*(hlinha + h2inha + L1/2);
npzo = cth*(h2inha + L1/2);
npxo = sth*(h2inha + L1/2);
zzc = [npzo; npz];
xxc = [npxo; npx];
LINHA=line(xxc, zzc,'Parent',EIXOS2,'Color',[0 0 0],'LineStyle','-', 'LineWidth',2);
if npz < 0, aquz = npz+0.06; else, aquz = npz-0.06; end
if npx < 0, aqux = npx+0.06; else, aqux = npx-0.06; end
text(aqux,aquz,'1','FontSize',10,'Parent',EIXOS2);
pz = [0 0.01 0.01 0 0];
px = [R2/2 R2/2 -R2/2 -R2/2 R2/2];
npzh = cth*pz - sth*px + npz;
npxh = sth*pz + cth*px + npx;
efe=patch(npxh, npzh,[0.9 0.9 0.9],'Parent',EIXOS2);
npz = sth*R2 + cth*(hlinha + h2inha + L1/2);
npx = -cth*R2 + sth*(hlinha + h2inha + L1/2);
npzo = cth*(h2inha + L1/2);
npxo = sth*(h2inha + L1/2);
zzc = [npzo; npz];
xxc = [npxo; npx];
LINHA=line(xxc, zzc,'Parent',EIXOS2,'Color',[0 0 0],'LineStyle','-', 'LineWidth',2);
if npz < 0, aquz = npz+0.06; else, aquz = npz-0.06; end
if npx < 0, aqux = npx+0.06; else, aqux = npx-0.06; end
text(aqux,aquz,'3','FontSize',10,'Parent',EIXOS2);
pz = [0 0.01 0.01 0 0];
px = [R2/2 R2/2 -R2/2 -R2/2 R2/2];
npzh = cth*pz - sth*px + npz;
npxh = sth*pz + cth*px + npx;
efe=patch(npxh, npzh,[0.9 0.9 0.9],'Parent',EIXOS2);
% Theta Angle
cade = theta*180/pi;
text(0.2,-0.2,'\theta = ','FontSize',12,'Parent',EIXOS2);
text(0,-0.2,sprintf('%2.2f ^{o}',cade),'FontSize',12,'Parent',EIXOS2);
% Local Frame
xxc = [0; 0];
yyc = [0; 0];
LINHA=line(yyc, xxc, 'Parent',EIXOS2,'Color',[0 0 1],'LineStyle','none', 'Marker','o');
LINHA=line(yyc, xxc, 'Parent',EIXOS2,'Color',[0 0 1],'LineStyle','none', 'Marker','.');