diff --git a/lib/ControlSystemsBase/src/pid_design.jl b/lib/ControlSystemsBase/src/pid_design.jl index a2376c6fd..81c0bbd52 100644 --- a/lib/ControlSystemsBase/src/pid_design.jl +++ b/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) diff --git a/lib/ControlSystemsBase/test/test_pid_design.jl b/lib/ControlSystemsBase/test/test_pid_design.jl index e9084ee84..4bab4aeea 100644 --- a/lib/ControlSystemsBase/test/test_pid_design.jl +++ b/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)