function yprime = wrist_model(t,y)
% WRIST_MODEL This function contains the diff eqns for the wrist hd.
% 	YPRIME = WRIST_MODEL(T,Y) calculates the differential equations
%	for the harmonic-drive robot in the wrist joint configuration.
%	This function can be used by the numerical integration routines
%	ODE23 and ODE45 to run a simulation of the dynamic equations.
%	This function takes the current time, T, and the state vector
%	Y = [wg_position wg_velocity fs_position fs_velocity] and
%	returns the derivative of the state vector, YPRIME, as
%	calculated by the system dynamic equations.
%
%	Note that this function was created to verify the simplified
%	set of differential equations derived for inclusion in the IEEE
%	Robotics and Automation paper.  The complete set of
%	differential equations can be found in the C program
%	hd_model.c.
%
%	See also ODE45, ODE23.

%	T.D. Tuttle 5-17-95
%	Copyright (c) 1995 by Timothy D. Tuttle and
%	Massachusetts Institute of Technology

% Define some useful constants.
global im;		% Motor current (set in the calling function)
deg2rad = (pi/180);	% Degrees to radians
rad2deg = 1/deg2rad;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Read in the wrist model parameters

% Geometry and trig identities:
alpha_tooth = 30*deg2rad;	  % Angle of the tooth (rad)
alpha_wg = 1.224989*deg2rad;	  % Angle of the wave-generator wedge (rad)
tan1 = tan(alpha_wg);
cos1 = cos(alpha_wg);
sin1 = sin(alpha_wg);
tan2 = tan(alpha_tooth);
cos2 = cos(alpha_tooth);
sin2 = sin(alpha_tooth);
N = (1/(tan1*tan2)) - 1;

% Testing station parameters:
J_in =  5.2e-6;		% Inertia at the wave-generator (kg*m^2/rad)
J_out = 0.0088;		% Inertia at the flexspline (kg*m^2/rad)
b_in =  0.0000001;	% Damping at the wave_generator (N*m/(wg_deg/sec))
b_out = 0.0005;		% Damping at the flexspline (N*m/(fs_deg/sec))
Kt =    0.0406;		% Motor torque constant (N*m/amp)

% Position Error:
A0 =  0.003*N*tan1;  	% Error amplitude 0 (deg at tooth)
ph0 = 0;		% Phase 0 (deg)
A1 =  0.015*N*tan1;  	% Error amplitude 1 (deg at tooth)
ph1 = 0;		% Phase 1 (deg)
A2 =  0.005*N*tan1;  	% Error amplitude 2 (deg at tooth)
ph2 = 90;		% Phase 2 (deg)

% Friction:
% Constant friction at tooth surface (N-m at surface)
b0 = 0.28937822173511135171;  
% Linear friction at tooth surface (N*m/(surface_deg/sec))
b1 = 0.00054700000000008009;
% Cubic friction at tooth surface (N*m/(surface_deg/sec)^3)
b2 = -0.00000000028160000000; 
% Cyclic friction amplitude at tooth surface (N-m at surface)
b_cyc = 0.00044 * 40.5;
% Cyclic friction phase (input deg)
b_ph = 180;
% Coulomb friction coefficient at tooth surface.
mu_save = 0.1;

% Stiffness:
k1 =  5.59403736006573915063;	% Linear stiffness (N*m/tooth_deg)
k2 = 10.92335688077296040888;	% Cubic stiffness (N*m/tooth_deg^3)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculate the harmonic-drive equations:

% Transfer the input vector to new variable names:
pos_wg = y(1);
vel_wg = y(2);
pos_fs = y(3);
vel_fs = y(4);

% Kinematic equations:
vel_wg_rel = vel_wg - vel_fs;
pos_wg_rel = pos_wg - pos_fs;
vel_ts = (1/sin2)*(-vel_fs);

% Kinematic-error equation:
pos_err = A0*sin((1*pos_wg_rel+ph0)*deg2rad) + ...
          A1*sin((2*pos_wg_rel+ph1)*deg2rad) + ...
          A2*sin((4*pos_wg_rel+ph2)*deg2rad);
vel_err = 1*deg2rad*A0*cos((1*pos_wg_rel+ph0)*deg2rad) + ...
          2*deg2rad*A1*cos((2*pos_wg_rel+ph1)*deg2rad) + ...
          4*deg2rad*A2*cos((4*pos_wg_rel+ph2)*deg2rad);

% Compliance equation:
pos_k = tan1*pos_wg_rel + pos_fs/tan2 + pos_err;
T_k = k1*pos_k + k2*(pos_k^3);

% Friction equations:
if vel_ts > 0,
  friction = b0 + b_cyc*sin((pos_fs + b_ph)*deg2rad);
else,
  friction = -b0 - b_cyc*sin((pos_fs + b_ph)*deg2rad);
  disp('NOTE: vel_ts is less than zero.');
end;

velmax = 1000;
if abs(vel_ts) > velmax,
  damping = b1*sign(vel_ts)*velmax + b2*((sign(vel_ts)*velmax)^3);
  disp('NOTE: vel_ts is greater than velmax.');
else
  damping = b1*vel_ts + b2*(vel_ts^3);
end

T_b = friction + damping;
if T_b<0, disp('Friction torque is negative!'); end;

% For the sake of continuity in the simulation, ramp the constant
% friction on the gear-tooth surface from zero to the desired value if at
% the beginning of the step-response trial.
if t < 0.05,
  T_b = (((t-0.05)/0.05)+1) * T_b;
end;

% Wave generator force-balance:
T_wg = (tan1 + vel_err)*T_k;

% Set the sign of mu correctly.
mu = sign(vel_ts)*mu_save;

% Calculate the normal and use it to set mu to zero if necessary.
% normal = (T_k - cos2*T_b)/(sin2 + mu*cos2);

normal_check = (T_k - cos2*T_b);
if normal_check < 0, disp('NOTE: mu has been set to zero.'); mu = 0; end;
T_fs = ((cos2 - mu*sin2)/(sin2 + mu*cos2))*T_k - ...
       (((cos2 - mu*sin2)*cos2/(sin2 + mu*cos2)) + sin2)*T_b - T_wg;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculate the differential equations and store in 'yprime'.
pos_wg_dot = vel_wg;
vel_wg_dot = (rad2deg/J_in)*(Kt*im - b_in*vel_wg - T_wg);
pos_fs_dot = vel_fs;
vel_fs_dot = (rad2deg/J_out)*(-b_out*vel_fs - T_fs);

yprime = [pos_wg_dot;
	  vel_wg_dot;
	  pos_fs_dot;
	  vel_fs_dot];