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 = 2000;           %Proportional gain (N/m);
   b = 0;
  
  
  Tmin = [50]*4.45;  %Min Tension Request (N) 
  Tmax = [500] *4.45;
  depth_error = TargetDepth-p(3);
  depth_error_rate = TargetDepthRate-v(3);

  T = k*depth_error+b*depth_error_rate;
  
  if T<Tmin  %Limit minimum tension request to try and keep cable taut.
    T = Tmin;
  end
  
  if T>Tmax
    T = Tmax;
  end

  