reorganized

git-svn-id: http://moon:8086/svn/projects/HendiControl@4 fda53097-d464-4ada-af97-ba876c37ca34

matlab/mass.m

@@ -0,0 +1,15 @@
+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