add loopshapingPD

lib/ControlSystemsBase/src/pid_design.jl

@@ -1,4 +1,4 @@
-export pid, pid_tf, pid_ss, pid_2dof, pid_ss_2dof, pidplots, leadlink, laglink, leadlinkat, leadlinkcurve, stabregionPID, loopshapingPI, placePI, loopshapingPID
+export pid, pid_tf, pid_ss, pid_2dof, pid_ss_2dof, pidplots, leadlink, laglink, leadlinkat, leadlinkcurve, stabregionPID, loopshapingPI, placePI, loopshapingPID, loopshapingPD
 export convert_pidparams_from_parallel, convert_pidparams_from_standard, convert_pidparams_from_to, convert_pidparams_to_parallel, convert_pidparams_to_standard
 
 """
@@ -519,6 +519,67 @@ function loopshapingPI(P0, ωp; ϕl=0, rl=0, phasemargin=0, form::Symbol=:standa
     (; C, kp, ki, fig, CF)
 end
 
+"""
+    C, kp, kd, fig, CF = loopshapingPD(P, ωp; ϕl, rl, phasemargin, form=:standard, doplot=false, Tf, F)
+
+Selects the parameters of a PD-controller (on parallel form) such that the Nyquist curve of `P` at the frequency `ωp` is moved to `rl exp(i ϕl)`
+
+The parameters can be returned as one of several common representations
+chosen by `form`, the options are
+* `:standard` - ``K_p(1 + 1/(T_i s) + T_d s)``
+* `:series` - ``K_c(1 + 1/(τ_i s))(τ_d s + 1)``
+* `:parallel` - ``K_p + K_i/s + K_d s``
+
+If `phasemargin` is supplied (in degrees), `ϕl` is selected such that the curve is moved to an angle of `phasemargin - 180` degrees
+
+If no `rl` is given, the magnitude of the curve at `ωp` is kept the same and only the phase is affected, the same goes for `ϕl` if no phasemargin is given.
+
+- `Tf`: An optional time constant for second-order measurement noise filter on the form `tf(1, [Tf^2, 2*Tf/sqrt(2), 1])` to make the controller strictly proper.
+- `F`: A pre-designed filter to use instead of the default second-order filter that is used if `Tf` is given.
+- `doplot` plot the `gangoffourplot` and `nyquistplot` of the system.
+
+See also [`loopshapingPI`](@ref), [`loopshapingPID`](@ref), [`pidplots`](@ref), [`stabregionPID`](@ref) and [`placePI`](@ref).
+"""
+function loopshapingPD(P0, ωp; ϕl=0, rl=0, phasemargin=0, form::Symbol=:standard, doplot=false, Tf = nothing, F=nothing)
+    issiso(P0) || throw(ArgumentError("P must be SISO"))
+    if F === nothing && Tf !== nothing
+        F = tf(1, [Tf^2, 2*Tf/sqrt(2), 1])
+    end
+    if F !== nothing
+        P = P0*F
+    else
+        P = P0
+    end
+    Pw = freqresp(P, ωp)[]
+    ϕp = angle(Pw)
+    rp = abs.(Pw)
+
+    if phasemargin > 0
+        ϕl == 0 || @warn "Both phasemargin and ϕl provided, the provided value for ϕl will be ignored."
+        ϕl = deg2rad(-180+phasemargin)
+    else
+        ϕl = ϕl == 0 ? ϕp : ϕl
+    end
+    rl = rl == 0 ? rp : rl
+
+    kp = rl/rp*cos(ϕp-ϕl)
+    kd = rl/(rp*ωp)*sin(ϕl-ϕp)
+    C = pid(kp, 0, kd, form=:parallel)
+    CF = F === nothing ? C : C*F
+
+    fig = if doplot
+        w = exp10.(LinRange(log10(ωp)-2, log10(ωp)+2, 500))
+        f1 = gangoffourplot(P0,CF, w)
+        f2 = nyquistplot([P0 * CF, P0], w, ylims=(-4,2), xlims=(-4,1.2), unit_circle=true, show=false, lab=["PC" "P"])
+        RecipesBase.plot!([rl*cos(ϕl)], [rl*sin(ϕl)], lab="Specification point", seriestype=:scatter)
+        RecipesBase.plot(f1, f2)
+    else
+        nothing
+    end
+    kp, _, kd = convert_pidparams_from_parallel(kp, 0, kd, form)
+    (; C, kp, kd, fig, CF)
+end
+
 
 """
     C, kp, ki = placePI(P, ω₀, ζ; form=:standard)

lib/ControlSystemsBase/test/test_pid_design.jl

@@ -142,6 +142,43 @@ _,_,_,pm = margin(P*CF)
 @test pm[] > 45*0.99
 
 
+# Test loopshapingPD
+P = tf(1,[1, 1, 1])
+
+# Basic phase-margin spec — mirrors the loopshapingPI block above
+C, kp, kd = loopshapingPD(P, 10; phasemargin = 30, doplot = false)
+_,_,_,pm = margin(P*C)
+@test pm[] > 30*0.99
+
+# With second-order noise filter Tf and doplot=true (exercises the plot branch and 5-tuple return)
+C, kp, kd, fig, CF = loopshapingPD(P, 1; phasemargin = 45, doplot = true, Tf = 0.1)
+_,_,_,pm = margin(P*CF)
+@test pm[] > 45*0.99
+
+# Design-point property: with default rl, L(jωp) has |P(jωp)| magnitude and angle -180°+phasemargin
+ωp = 2.0
+pm_spec = 60
+C, kp, kd, fig, CF = loopshapingPD(P, ωp; phasemargin = pm_spec)
+L_at_ωp = freqresp(P*CF, ωp)[]
+@test abs(L_at_ωp) ≈ abs(freqresp(P, ωp)[]) rtol=1e-6
+@test rad2deg(angle(L_at_ωp)) ≈ -180 + pm_spec rtol=1e-6
+
+# Explicit rl, ϕl spec — verifies the rl/ϕl branch
+rl_spec = 0.5
+ϕl_spec = deg2rad(-150)
+C, kp, kd = loopshapingPD(P, ωp; rl = rl_spec, ϕl = ϕl_spec)
+L_at_ωp = freqresp(P*C, ωp)[]
+@test abs(L_at_ωp) ≈ rl_spec rtol=1e-6
+@test angle(L_at_ωp) ≈ ϕl_spec rtol=1e-6
+
+# SISO guard
+@test_throws ArgumentError loopshapingPD(ssrand(2,2,2), 1.0)
+
+# form = :parallel returns kp, kd that reconstruct C directly
+C, kp, kd = loopshapingPD(P, ωp; phasemargin = 45, form = :parallel)
+@test freqresptest(C, pid(kp, 0, kd, form=:parallel)) < 1e-10
+
+
 # Test placePI
 P = tf(1,[1, 1])
 C, Kp, Ti = placePI(P, 2, 0.7; form=:standard)