From 95798edc614268ed49d0c0a3deb117adbefef585 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Thu, 29 Jun 2023 10:55:20 +0200
Subject: [PATCH 1/2] better handling of types and test fixed-point arithmetics

---
 Project.toml        |  7 +++----
 src/DiscretePIDs.jl | 24 ++++++++++++------------
 test/runtests.jl    | 11 +++++++++++
 3 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/Project.toml b/Project.toml
index 9143f67..0b17757 100644
--- a/Project.toml
+++ b/Project.toml
@@ -1,16 +1,15 @@
 name = "DiscretePIDs"
 uuid = "c1363496-6848-4723-8758-079b737f6baf"
 authors = ["Fredrik Bagge Carlson"]
-version = "0.1.1"
-
-[deps]
+version = "0.1.2"
 
 [compat]
 julia = "1.7"
 
 [extras]
 ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e"
+FixedPointNumbers = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "ControlSystems"]
+test = ["Test", "ControlSystems", "FixedPointNumbers"]
diff --git a/src/DiscretePIDs.jl b/src/DiscretePIDs.jl
index 487bd0e..74ff495 100644
--- a/src/DiscretePIDs.jl
+++ b/src/DiscretePIDs.jl
@@ -51,19 +51,19 @@ U(s) = K \\left( bR(s) - Y(s) + \\dfrac{1}{sT_i} \\left( R(s) Y(s) \\right) - \\
 See also [`calculate_control`](@ref), [`set_K!`](@ref), [`set_Ti!`](@ref), [`set_Td!`](@ref)
 """
 function DiscretePID(;
-    K  = 1f0,
+    K::T  = 1f0,
     Ti = false,
     Td = false,
-    Tt = Ti > 0 && Td > 0 ? √(Ti*Td) : 10,
-    N  = 10f0,
-    b  = 1f0,
-    umin = -float(typeof(K))(Inf),
-    umax = float(typeof(K))(Inf),
+    Tt = Ti > 0 && Td > 0 ? typeof(K)(√(Ti*Td)) : typeof(K)(10),
+    N  = typeof(K)(10),
+    b  = typeof(K)(1),
+    umin = typemin(K),
+    umax = typemax(K),
     Ts,
-    I    = 0.0f0,
-    D    = 0.0f0,
-    yold = 0.0f0,
-)
+    I    = zero(typeof(K)),
+    D    = zero(typeof(K)),
+    yold = zero(typeof(K)),
+) where T
     if Ti > 0
         bi = K * Ts / Ti
     else
@@ -79,9 +79,9 @@ function DiscretePID(;
     ad = Td / (Td + N * Ts)
     bd = K * N * ad
 
-    T = promote_type(typeof.((K, Ti, Td, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
+    T2 = promote_type(typeof.((K, Ti, Td, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
 
-    DiscretePID(T.((K, Ti, Td, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
+    DiscretePID(T2.((K, Ti, Td, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
 end
 
 """
diff --git a/test/runtests.jl b/test/runtests.jl
index 59d4adc..77b45a9 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -67,6 +67,16 @@ res2 = lsim(P, ctrl, Tf)
 @test res.y ≈ res2.y rtol=0.02
 # plot([res, res2])
 
+## Test with FixedPointNumbers
+using FixedPointNumbers
+T = Fixed{Int16, 10} # 16-bit signed fixed-point with 11 bits for the fractional part
+pid = DiscretePID(; K = T(K), Ts = T(Ts), Ti = T(Ti), Td = T(Td))
+@test pid isa DiscretePID{T}
+
+res3 = lsim(P, ctrl, Tf)
+
+@test res.y ≈ res3.y rtol=0.05
+
 
 ## PI control with sp weighting
 Tf = 10
@@ -139,4 +149,5 @@ res2 = lsim(P, ctrl, 3)
 @test DiscretePID(Ts=1f0) isa DiscretePID{Float32}
 @test DiscretePID(Ts=1.0) isa DiscretePID{Float64}
 
+
 end
\ No newline at end of file

From cc3dc41b7358969aa1e1c9493dfbc7b3de5e2c4f Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Thu, 29 Jun 2023 10:55:27 +0200
Subject: [PATCH 2/2] add fixed-point demo

---
 README.md | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/README.md b/README.md
index 832df31..cab5ef2 100644
--- a/README.md
+++ b/README.md
@@ -116,6 +116,19 @@ The figure should look more or less identical to the one above, except that we p
 - Bumpless transfer when updating `K` is realized by updating the state `I`. See the docs for `set_K!` for more details.
 - The total control signal $u(t)$ (PID + feed-forward) is limited by the integral anti-windup.
 
+## Simulation of fixed-point arithmetic
+If the controller is ultimately to be implemented on a platform without floating-point hardware, you can simulate how it will behave with fixed-point arithmetics using the `FixedPointNumbers` package. The following example modifies the first example above and shows how to simulate the controller using 16-bit fixed-point arithmetics with 10 bits for the fractional part:
+```julia
+using FixedPointNumbers
+T = Fixed{Int16, 10} # 16-bit fixed-point with 10 bits for the fractional part
+pid = DiscretePID(; K = T(K), Ts = T(Ts), Ti = T(Ti), Td = T(Td))
+res_fp = lsim(P, ctrl, Tf)
+plot([res, res_fp], plotu=true, lab=["Float64" "" string(T) ""]); ylabel!("u + d", sp=2)
+```
+![Fixed-point simulation result](https://user-images.githubusercontent.com/3797491/249722782-2157d625-7eb0-4f77-b630-69199237f164.png)
+
+The fixed-point controller behaves roughly the same in this case, but artifacts are clearly visible. If the number of bits used for the fractional part is decreased, the controller will start to misbehave.
+
 ## See also
 - [TrajectoryLimiters.jl](https://github.com/baggepinnen/TrajectoryLimiters.jl) To generate dynamically feasible reference trajectories with bounded velocity and acceleration given an instantaneous reference $r(t)$ which may change abruptly.
 - [SymbolicControlSystems.jl](https://github.com/JuliaControl/SymbolicControlSystems.jl) For C-code generation of LTI systems.
\ No newline at end of file