-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathvandpol2.m
141 lines (111 loc) · 2.59 KB
/
vandpol2.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
clear all
global mu SimulationInput BSM BSF JM JG1 JF g Gc GR L R KE KF KG KI KR KS KT lF mF ctrl phi_ref time2 f theta_u
BSM = 0.01;
BSF = 1.5;
JM = 0.002;
JG1 = 0.001;
JF = 0.0204;
g = 9.81 ;
Gc = 6.1;
GR = 1.6;
L = 0.1 ;
R = 4 ;
KE = 0.35;
KF = 0.5;
KG = 2.0;
%KI = 0 ;
KR = 1.5;
KS = 1.5;
KT = 0.35;
lF = 0.35 ;
mF = 0.5 ;
phi_ref=0.785398163;
theta_u=0.087266462;
SimulationInput = 0;
mu = 1;
% Define parameters for the simulation
stepsize = 0.001; % Integration step size
comminterval = 0.04; % Communications interval
EndTime = 150; % Duration of the simulation (final time)
i = 0; % Initialise counter for data storage
% Initial conditions of all states and state derivatives
x = [0,0,0,0,0,0.087266462,0]';
xdot = [0,0,0,0,0,0,0]';
%
% END OF INITIAL SEGMENT - all parameters initialised
%
% DYNAMIC SEGMENT
%interpolation data import
M=csvread('interpol_data.txt');
r=M(:,1);
c=M(:,2);
index=1;
time2=r.';
f=c.';
f=f.*3.1415/180;
for time = 0:stepsize:EndTime,
% store time state and state derivative data every communication interval
if rem(time,comminterval)==0
i = i+1; % increment counter
tout(i) = time; % store time
xout(i,:) = x; % store states
xdout(i,:) = xdot; % store state derivatives
end % end of storage
%need controller
Gc = 0.175;
KI = 0.827;
theta_u=interpolation(time);
delta = KR*phi_ref - KS*x(3);
ctrl=Gc*(delta+KI*eulerint(delta,stepsize, delta));
% DERIVATIVE SECTION
xdot = model3(x,SimulationInput);
% INTEG SECTION
x = rk4int('model3',stepsize,x,SimulationInput);
%x=eulerint(xdot,stepsize,x);
% END OF INTEG SECTION
end
% END OF DYNAMIC SEGMENT
%
% TERMINAL SEGMENT
figure(1)
hold on
clf
plot(tout,xout(:,1),'b-')
xlabel('time [s]')
ylabel('Current [A]')
figure(2)
hold on
clf
plot(tout,xout(:,2),'b-')
xlabel('time [s]')
ylabel('Actuator angular velocity [rad/s]')
figure(3)
hold on
clf
plot(tout,xout(:,3),'b-')
xlabel('time [s]')
ylabel('Actuator deflection angle [rad]')
figure(4)
hold on
clf
plot(tout,xout(:,4),'b-')
xlabel('time [s]')
ylabel('Gear angular velocity [rad/s]')
figure(5)
hold on
clf
plot(tout,xout(:,5),'b-')
xlabel('time [s]')
ylabel('Forearm angular velocity [rad/s]')
figure(6)
hold on
clf
plot(tout,xout(:,6),'b-')
xlabel('time [s]')
ylabel('Forearm deflection angle [rad]')
figure(7)
hold on
clf
plot(tout,xout(:,7),'b-')
xlabel('time [s]')
ylabel('Gear deflection angle [rad]')