-
Notifications
You must be signed in to change notification settings - Fork 1
/
System.m
297 lines (242 loc) · 12.9 KB
/
System.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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
%============================================%
% Choose Motors %
%============================================%
% Motor Unit Conversions
% ----------------------
% Of all the 25 combinations of motors, the following combo produces a
% scenario where the settle time is small and the peak is reasonable for
% both the motors. Although the settle time for Q0 is not the smallest
% of the combinations, its peak value is
%
AMAX22_6W_SB;
Q0 = MotorParam;
AMAX12_p75W_SB;
Q1 = MotorParam;
%============================================%
% Motor Parameters %
%============================================%
% Q(n) refers to element at index=n of MotorParam.
% Parameters found in the AMAX22_5W_SB motor file, and the corresponding indices to the parameters are found in CONSTANTS.m
% Q0
NomI0 = Q0(NomCurr); % Max average current
StallI0 = Q0(StallCurr); % Max peak current
NomQ0 = Q0(NomV); % Nominal voltage
% Q1
NomI1 = Q1(NomCurr); % Max average current
StallI1 = Q1(StallCurr); % Max peak current
NomQ1 = Q1(NomV); % Nominal voltage
%============================================%
% AmpDynLifier Dynamics %
%============================================%
% Compute transfer function of AmpDynLifier. R,L,C values are defined in CONSTANTS.m
AmpDynR1 = R1*10^6; % Mohm->ohm
AmpDynC = C/10^6; % uF->F
AmpDynL = L/10^3; % mH->H
% The AmpDynLifier dynamics will be the same for both the motors since they use the same AmpDynLifier circuitry
% Q0
Amp0n = [AmpDynC*AmpDynR1*R2-AmpDynL]; % Numerator (C*R1*R2 - L)
Amp0d = [AmpDynL*AmpDynC*AmpDynR1 AmpDynC*AmpDynR1*R2] % Denominator ((L*C*R1)s + (C*R1*R2))
AmpSat0 = NomQ0; % Set such that the maximum motor voltage is not exceeded (Nominal Voltage)
% Q1
Amp1n = [AmpDynC*AmpDynR1*R2-AmpDynL]; % Numerator (C*R1*R2 - L)
Amp1d = [AmpDynL*AmpDynC*AmpDynR1 AmpDynC*AmpDynR1*R2] % Denominator ((L*C*R1)s + (C*R1*R2))
AmpSat1 = NomQ1; % Set such that the maximum motor voltage is not exceeded (Nominal Voltage)
%============================================%
% System Parameters %
%============================================%
% Some values that will be used in the calculations of the mechanical dynamics
% Mass of system (motors and ring)
% --------------------------------------------
% The values for the motors are given, so just the mass of the ring needs to be calculated.
% Mass = volume*density
Rin = LinkR1/10^3; % Inner radius of wrist frame, mm->m
Rout = LinkR2/10^3; % Outer radius of wrist frame, mm->m
Depth = LinkD/10^3; % Depth of wrist frame, mm->m
RCenter = LinkOff/10^3; % Distance from the motor face to the center, mm->m
LengthQ0 = Q0(Length)/10^3; % Length of motor Q0, mm->m
LengthQ1 = Q1(Length)/10^3; % Length of motor Q1, mm->m
Ringvol = pi*(Rout^2-Rin^2)*Depth; % Volume of the ring
RingDensity = RhoAl*10^3; % Density of 6061 Al, g/cm^3->kg/m^3: g/cm^3*(1kg/1000g)*(100^3cm^3/1m^3)=kg/m^3*10^3
MassRing = Ringvol*RingDensity; % Mass of ring = volume * density
MassQ0 = Q0(Weight)/10^3; % Mass of motor Q0, g->kg
MassQ1 = Q1(Weight)/10^3; % Mass of motor Q1, g->kg
TotalMass = 2*MassQ1 + MassRing; % Total mass that is being supported by Q0. It is assumed that the counterweight for Q1 possesses the same mass as Q1
% --------------------------------------------
%============================================%
% Q0 - Rotation about y-axis %
%============================================%
% Electrical Motor Dynamics
% --------------------------------------------
% The armature admittance is the electrical component of motor response.
% The motors eletrical characteristics can be modeled with an armature circuit,
% where the terminal impedance is represented as a resistor and inductor in series.
% Z = sLa + Ra -> Y=1/(sLa + Ra)
Ra0 = Q0(TermR); % Terminal (armature) resistance
La0 = Q0(TermL)/10^3; % Terminal (armature) inductance, mH->H
Elec0n = 1; % Numerator (s)
Elec0d = [La0 Ra0]; % Denominator (sL + R)
% --------------------------------------------
% Torque Const & Back EMF
% --------------------------------------------
TConst0 = Q0(TorqueK)/10^3; % Torque constant, mNm/A->Nm/A
BackEMF0 = 1/(Q0(SpdK)*RadPSecPerRPM); % SpdK is the speed constant in rpm/v, convert to Vs/rad: rpm/V*RadPSecPerRPM = Volt*seconds/rad
% --------------------------------------------
% --------------------------------------------%
% Mechanical Motor Dynamics %
% --------------------------------------------%
% The transfer function of the motor is s/(Js^2 + Bs + K)
% where J is the inertia, B is the damping coefficent, and K is the dynamic friction
% J units: Nms^2/rad
% B units: Nms/rad
% K units: Nm/rad
% J: Moment of Inertia
% --------------------------------------------
% The total moment of inertia associated with motor Q0 is J0 = J0Ring + J0Internal + J0MotorQ1
% J0Ring is the moment of inertia of the ring, J0Internal is the moment of inertia of motor Q0,
% and J0MotorQ1 is the moment of inertia of motor Q1 with respect to motor Q0
% J0Internal: rotational inertia of Q0, which is a given parameter.
J0Internal = Q0(RotJ)/10^7; % Internal Motor Inertia, gcm^2->kgm^2: gcm^2*(1kg/1000 g)*(1m^2/100^2cm^2)=kgm^2/10^7
% J0Ring: the moment of inertia (about y axis) for a hollow cylinder about the y-axis is given by:
Din = 2*Rin; % Inner diameter of wrist frame
Dout = 2*Rout; % Outer radius of wrist frame
J0Ring = (1/4)*MassRing*((Din^2+Dout^2)/4 +(Depth^2)/3); % Inertia of Ring, Moment of Inertia Calculation for a Hollow Ring
% J0MotorQ1: the moment of inertia of motor Q1 (about y axis) with respect to motor Q0
% The inertia will be calculated for two cylinders separately. One cylinder will extend from the end of the motor Q1 to the centre
% of the ring (Cylinder 1). The other cylinder will extend from the start of motor Q1 to the centre of the ring. The substraction
% of these inertias will give the inertia of a the motor Q1 without the counterweight. By assuming that the counterweight has the
% same mass as motor Q1, this result can be multiplied by 2 to get the total inertia of the motor Q1 system about the y axis.
M10 = MassQ1 + (RCenter/LengthQ1)*MassQ1; % Mass of Cylinder 1 (calculated assuming uniform density in motor Q1)
M20 = (RCenter/LengthQ1)*MassQ1 % Mass of Cylinder 2 (calculated assuming uniform density in motor Q1)
% Inertia about y axis with respect to the end of a cylinder
% Iend = mL^2/3
J10 = M10*(LengthQ1 + RCenter)^2/3; % Inertia of Cylinder 1 about the y-axis
J20 = M20*(RCenter)^2/3; % Inertia of Cylinder 2 about the y-axis
J0MotorQ1 = (J10 - J20) * 2; % Total inertia of motor Q1 system about the y axis
% J0
% The mass of the parts in which the ring and the motor Q1 system overlap will be accounted for
% twice, but this is negligible
J0 = J0Ring + J0Internal + J0MotorQ1; % units: Nms^2/rad
% --------------------------------------------
% B: Damping Coefficient
% --------------------------------------------
INoLoad0 = 2*Q0(NoLoadCurr)/10^3; % mA->
SNoLoad0 = Q0(NoLoadSpd)*RadPSecPerRPM; % rpm->rad/s
B0 = (INoLoad0*TConst0)/SNoLoad0; % units: Nms/rad
% --------------------------------------------
% K: Dynamic friction constant
% --------------------------------------------
K0 = (SpringK/10^3)/(2*pi); % Spring constant, mNm/rev->Nm/rad
% --------------------------------------------
% Mechanical Dynamics Vectors
% --------------------------------------------
Mech0n = [1 0]; % Numerator (s)
Mech0d = [J0 B0 K0]; % Denominator (Js^2 + Bs + K)
JntSat0 = Big; % Motor 0 does not have an angle limit
% --------------------------------------------
% Sensor Dynamics
% --------------------------------------------
Sens0 = SensV/(SensAng*RadPerDeg); % SensAng is in deg so convert to rad
SensSat0 = SensV;
% --------------------------------------------
% Static Friction
% --------------------------------------------
% Fs = us*Ns where us is the coefficient of static friction and Ns is the normal force.
% Since motor Q0 controls the ring, the weight associated with the friction (Ns, the normal force)
% will be the sum of the weights of the motors and the ring.
% Weight = Weight of Q0 + Weight of Q1 + Weight of ring -> Ns = Weight = Mass*g
% The total mass was calculated at the beginning of the file
TotalWeight = TotalMass*G; % G is the gravitational acceleration
StFric0 = uSF*TotalWeight/10^6; % Fs = us*Ns, uNm->Nm, /2 since the counter weight exerts a normal force of 1/2 of the total weight
% --------------------------------------------
%============================================%
% Q1 - Rotation about x-axis %
%============================================%
% Electrical Motor Dynamics
% --------------------------------------------
Ra1 = Q1(TermR); % Terminal (armature) resistance
La1 = Q1(TermL)/10^3; % Terminal (armature) inductance, mH->H
Elec1n = 1; % Numerator (1)
Elec1d = [La1 Ra1]; % Denominator (sL + R)
% ---------------------
% Torque Const & Back EMF
% --------------------------------------------
TConst1 = Q1(TorqueK)/10^3; % Torque constant, mNm/A->Nm/A
BackEMF1 = 1/(Q1(SpdK)*RadPSecPerRPM); % SpdK is the speed constant in rpm/v, convert to Vs/rad: rpm/V*RadPSecPerRPM = Volt*seconds/rad
% --------------------------------------------
% Mechanical Motor Dynamics
% --------------------------------------------
% J: Moment of Inertia
% --------------------------------------------
% The total moment of inertia associated with motor Q1 is J1 = J1Internal
% J1Internal is given by just the rotational inertia of Q1, which is a given parameter
J1Internal = Q1(RotJ)/10^7; % gcm^2->kgm^2: gcm^2*(1kg/1000 g)*(1m^2/100^2cm^2)=kgm^2/10^7
J1 = J1Internal; % units: Nms^2/rad
% --------------------------------------------
% B: Damping Coefficient
% --------------------------------------------
INoLoad1 = Q1(NoLoadCurr)/10^3; % mA->
SNoLoad1 = Q1(NoLoadSpd)*RadPSecPerRPM; % rpm->rad/s
B1 = (INoLoad1*TConst1)/SNoLoad1; % units: Nms/rad
% --------------------------------------------
% Mechanical Dynamics Vectors
% --------------------------------------------
Mech1n = [1]; % Numerator (1<)
Mech1d = [J1 B1]; % Denominator (Js + Bs)
% Transfer function:
% Normalized transfer function:
JntSat1 = JntLim*RadPerDeg; % Joint saturation is the angle limit of motor Q1, deg->rad
% --------------------------------------------
% Sensor Dynamics
% --------------------------------------------
Sens1 = SensV/(SensAng*RadPerDeg); % SensAng is in deg so convert to rad
SensSat1 = SensV;
% --------------------------------------------
% Static Friction
% --------------------------------------------
StFric1 = 0; % None associated with motor Q1
% --------------------------------------------
%============================================%
% Q0 Transfer Functions
%============================================%
A0 = tf(Amp0n,Amp0d); % AmpDynLifier
E0 = tf(Elec0n,Elec0d); % Electrical Motor Dynamics
M0 = tf(Mech0n,Mech0d); % Mechanical Motor Dynamics
%============================================%
% Q1 Transfer Functions %
%============================================%
A1 = tf(Amp1n,Amp1d); % AmpDynLifier
E1 = tf(Elec1n,Elec1d); % Electrical Motor Dynamics
M1 = tf(Mech1n,Mech1d); % Mechanical Motor Dynamics
%============================================%
% Q1 Transfer Functions %
%============================================%
INT = tf(1,[1 0]); % integrator: 1/s
% Q0
% G0_1 = E0*TConst0*M0;
% T0_1 = feedback(G0_1,BackEMF0);
% % Open loop gain for Q0
% GH0_zpk = zpk(A0*T0_1*INT);
% GH0 = minreal(GH0_zpk);
% dcgain0 = dcgain(GH0);
% GH0_norm = GH0/dcgain0;
% % pid_num0 = [1.2815e+08];
% % pid_den0 = [1, 15279.1702128,748862.340426,0];
% % GH0_pid = zpk(tf(pid_num0,pid_den0));
% T0 = feedback(GH0,1);
T0_1 = feedback(M0,StFric0);
T0_2 = E0*TConst0*T0_1;
T0_3 = feedback(T0_2,BackEMF0);
% Open loop gain for Q0
T0_4 = A0*T0_3*INT;
GH0_zpk = zpk(A0*T0_3*INT);
GH0 = minreal(GH0_zpk);
KDC0 = dcgain(GH0_zpk);
GH0_norm = GH0/KDC0;
% Open loop gain for Q1
T1_1 = E1*TConst1*M1;
T1_2 = feedback(T1_1,BackEMF1);
T1_3 = A1*T1_2*INT;
GH1_zpk = zpk(A1*T1_2*INT);
GH1 = minreal(GH1_zpk);
KDC1 = dcgain(GH1);
GH1_norm = GH1/KDC1;