jens
c4e48007f1
git-svn-id: http://moon:8086/svn/projects/HendiControl@4 fda53097-d464-4ada-af97-ba876c37ca34
16 lines
318 B
Matlab
16 lines
318 B
Matlab
function [theta, s] = mass(si, dt, C, M, L, P, Td, dTheta)
|
|
s = si;
|
|
if ~isstruct(si)
|
|
alpha = 1.0;
|
|
if Td > 0
|
|
alpha = dt/Td;
|
|
end
|
|
s = struct('e', 0, 'a', alpha, 'x', 0, 'gain', 0.8);
|
|
end
|
|
|
|
s.e = s.e * (1-((L*(0+dTheta))*dt)/(M*C));
|
|
s.x = (1-s.a)*s.x + s.gain*s.a*P*dt;
|
|
s.e = s.e + s.x;
|
|
theta = s.e/(M*C);
|
|
endfunction
|