DK = 0.5;            % Constant for P term of PID
DTi = 50000.0;          % Time constant for I term of PID (in denominator; bigger values less I term in output
DTd = 500.0;          % Time constant for D term of PID
DTf = 2.0;          % Derivative filter time constant
DTt = 100.0;          % Anti-windup time constant
DB = 1.0;            % Setpoint weight
DuLow = -3000/60.0;      % minimum possible output (RPS)
DuHigh = 3000/60.0;    % maximum possible output (RPS)
Dh = 3.0;            % Sample period (seconds)
COUNTSperREV = 500.0 * 4.0;

Dden = (DTf * DTf) + (2 * Dh * DTf) + (2 * Dh * Dh);   % denominator of measured signal filter terms
Dp1 = DTf * DTf / Dden;                             % first part of state variable x2 equation
Dp2 = 2 * Dh * Dh / Dden;                           % second part of state variable x1 equation
Dp3 = DK * Dh / DTi;                                % integral gain
Dp4 = DK * DTd / Dh;                                % derivative gain
Dp5 = Dh / DTt; 

depthSetPoint = -5.0;
currentDepth = -5.7;

Dy2 = 0.0;
Dy1 = 0.0;
DI = 0.0;
%Astrom & Hagglund PID Algorithm
Dy2 = (Dp1 * Dy2) + (Dp2 * (currentDepth - Dy1));     % update filter state y2
Dy1 = Dy1 + Dy2;                                    % update filter state y1
Dv = DK * ((DB * depthSetPoint) - Dy1) - (Dp4 * Dy2) + DI;            % compute nominal output
         
% limit the nominal output to desired range
if (Dv < DuLow)
    Du = DuLow;
else if (Dv > DuHigh)
     Du = DuHigh;
     elseDp4
     Du = Dv;
    end
end

 % update the integral
 DI = DI + (Dp3 * (depthSetPoint - Dy1)) + (Dp5 * (Du - Dv));

 %Convert Rev per second to Counts per second
 DuCPS = (Du * COUNTSperREV);
 %Need a -1 to get the extend retract direction correct
 DuCPS = -1 * DuCPS;
