function T = TensionCommand2(p,v,TetherOut,TetherRate,t)

% This function implements a depth control loop for the ESP by returning a
% tension (torque) target for the motor controller based upon the current depth of
% the ESP and the target depth.
% Arguments:    p = 6 dof position of the ESP relative to the global coordinate system.
%               v = 6 dof velocity of the ESP relative to the global coordinate system.
%               TetherOut = current amount of tether payed out.
%               TetherRate = current payout speed.  Positive equal payout.
%               t = current time in seconds.

  [TargetDepth TargetDepthRate] = TargetDepth_Profiling(t);  %Get Target depth for this time.

   k = 3000;           %Proportional gain (N/m);
   b = 1000;
  
  gTC_k = k;
  gTC_b  = b;
  
  Tmin = [50]*4.45;  %Min Tension Request (N) 
  depth_error = TargetDepth-p(3);
  depth_error_rate = TargetDepthRate-v(3);

  T = k*depth_error+b*depth_error_rate;
  
  if T<Tmin  
    T = Tmin;
  end