From f96fe55327e3a191eba97713b7ef653e9dee2e08 Mon Sep 17 00:00:00 2001 From: Brian Estevez Date: Wed, 15 Mar 2017 16:43:03 -0400 Subject: [PATCH 1/6] PD & controlLoop --- src/com/nutrons/framework/util/FlowOperators.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 250b9d3..5742e21 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -97,12 +97,25 @@ public static FlowableTransformer pidLoop(double proportional, public static FlowableTransformer pdLoop(double proportional, double derivative) { + return controlLoop(proportional, derivative, 0.0, error -> Flowable.just(0.0)); + } + + private static FlowableTransformer controlLoop(double proportional, + double derivative, + double integral, + Function, Flowable> errorI) { return error -> { Flowable errorP = error.map(x -> x * proportional); Flowable errorD = error.buffer(2, 1) .map(last -> last.stream().reduce(0.0, (x, y) -> x - y)) .map(x -> x * derivative); - return Flowable.combineLatest(errorP, errorD, (p, d) -> p + d).share(); + try { + return Flowable.combineLatest(errorP, errorI.apply(error).map(x -> x * integral), errorD, + (p, i, d) -> p + i + d); + } catch (Exception e) { + e.printStackTrace(); + throw new RuntimeException(e); + } }; } From 39a65f107a5109e1a7fa7bd62950ba93feb3665c Mon Sep 17 00:00:00 2001 From: Brian Estevez Date: Wed, 15 Mar 2017 17:21:29 -0400 Subject: [PATCH 2/6] changed PID loop --- .../nutrons/framework/util/FlowOperators.java | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 5742e21..f2bd009 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -81,18 +81,10 @@ public static FlowableTransformer pidLoop(double proportional, int integralBuffer, double integral, double derivative) { - return error -> { - Flowable errorP = error.map(x -> x * proportional); - Flowable errorI = error.buffer(integralBuffer, 1) - .map(list -> list.stream().reduce(0.0, (x, acc) -> x + acc)) - .map(x -> x * integral); - Flowable errorD = error.buffer(2, 1) - .map(last -> last.stream().reduce(0.0, (x, y) -> x - y)) - .map(x -> x * derivative); - Flowable output = Flowable.combineLatest(errorP, errorI, errorD, - (p, i, d) -> p + i + d); - return output; - }; + return controlLoop(proportional, derivative, integral, + (error) -> error.buffer(integralBuffer, 1) + .map(list -> list.stream().reduce(0.0, (x, acc) -> x + acc)) + .map(x -> x * integral/integralBuffer)); } public static FlowableTransformer pdLoop(double proportional, From d586354252935efe1248e709c27b5a054f4db84e Mon Sep 17 00:00:00 2001 From: Brian Estevez Date: Wed, 15 Mar 2017 17:53:27 -0400 Subject: [PATCH 3/6] Added Exponential PID loop --- .../nutrons/framework/util/FlowOperators.java | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index f2bd009..dc116ee 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -20,10 +20,10 @@ public class FlowOperators { * Generate a Flowable from a periodic call to a Supplier. Drops on backpressure. * * @param ignored the number of time units to wait before calling the supplier again - * @param the type of the Flowable and Supplier + * @param the type of the Flowable and Supplier */ public static Flowable toFlow(Supplier supplier, - long ignored, TimeUnit unit) { + long ignored, TimeUnit unit) { return Flowable.interval(ignored, unit).subscribeOn(Schedulers.io()) .map(x -> supplier.get()).onBackpressureDrop().observeOn(Schedulers.computation()) .onBackpressureDrop().share(); @@ -57,7 +57,7 @@ public static Flowable deadband(Flowable input) { * specified by minimum and maximum. If so, the value will be changed to remap. */ public static Function deadbandMap(double minimum, double maximum, - double remap) { + double remap) { return bandMap(minimum, maximum, x -> remap); } @@ -66,7 +66,7 @@ public static Function deadbandMap(double minimum, double maximu * specified by minimum and maximum. If so, the value will be passed through the remap function. */ public static Function bandMap(double minimum, double maximum, - Function remap) { + Function remap) { return x -> x < maximum && x > minimum ? remap.apply(x) : x; } @@ -78,24 +78,33 @@ public static T getLastValue(Flowable input) { * Creates a PID Loop Function. */ public static FlowableTransformer pidLoop(double proportional, - int integralBuffer, - double integral, - double derivative) { + int integralBuffer, + double integral, + double derivative) { return controlLoop(proportional, derivative, integral, (error) -> error.buffer(integralBuffer, 1) .map(list -> list.stream().reduce(0.0, (x, acc) -> x + acc)) - .map(x -> x * integral/integralBuffer)); + .map(x -> x * integral / integralBuffer)); + } + + public static FlowableTransformer exponentialPidLoop(double proportinal, + double integralBuffer, + double integral, + double derivative) { + return controlLoop(proportinal, integral, derivative, error -> error.scan( + (newVal, lastAvg) -> lastAvg * (integralBuffer - 1) / integralBuffer + 1 / integralBuffer)); + } public static FlowableTransformer pdLoop(double proportional, - double derivative) { + double derivative) { return controlLoop(proportional, derivative, 0.0, error -> Flowable.just(0.0)); } private static FlowableTransformer controlLoop(double proportional, - double derivative, - double integral, - Function, Flowable> errorI) { + double derivative, + double integral, + Function, Flowable> errorI) { return error -> { Flowable errorP = error.map(x -> x * proportional); Flowable errorD = error.buffer(2, 1) From adce4bbfb57454587297b242f6a0a22b2a3ca909 Mon Sep 17 00:00:00 2001 From: Brian Estevez Date: Thu, 16 Mar 2017 18:32:06 -0400 Subject: [PATCH 4/6] Added Java Docs --- .../nutrons/framework/util/FlowOperators.java | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index dc116ee..90bb80a 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -76,8 +76,10 @@ public static T getLastValue(Flowable input) { /** * Creates a PID Loop Function. + * DON'T USE EVER */ - public static FlowableTransformer pidLoop(double proportional, + @Deprecated + public static FlowableTransformer veryBadDontUseEverPidLoop(double proportional, int integralBuffer, double integral, double derivative) { @@ -87,20 +89,28 @@ public static FlowableTransformer pidLoop(double proportional, .map(x -> x * integral / integralBuffer)); } + /** + * Exponential average version of PID Loop. + */ public static FlowableTransformer exponentialPidLoop(double proportinal, double integralBuffer, double integral, double derivative) { return controlLoop(proportinal, integral, derivative, error -> error.scan( - (newVal, lastAvg) -> lastAvg * (integralBuffer - 1) / integralBuffer + 1 / integralBuffer)); - + (newVal, lastAvg) -> lastAvg * (integralBuffer - 1) / integralBuffer + newVal / integralBuffer)); } + /** + * RegularPD loop with the 0.0 integral stream. + */ public static FlowableTransformer pdLoop(double proportional, double derivative) { return controlLoop(proportional, derivative, 0.0, error -> Flowable.just(0.0)); } + /** + * Control Loop for PID which + */ private static FlowableTransformer controlLoop(double proportional, double derivative, double integral, From 225330a8353543f2b80f7e86e0ebf4ea6fb86395 Mon Sep 17 00:00:00 2001 From: Brian Estevez Date: Thu, 16 Mar 2017 19:23:06 -0400 Subject: [PATCH 5/6] Fixed test checks --- test/com/nutrons/framework/test/TestPID.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/com/nutrons/framework/test/TestPID.java b/test/com/nutrons/framework/test/TestPID.java index 46bb60f..5b75440 100644 --- a/test/com/nutrons/framework/test/TestPID.java +++ b/test/com/nutrons/framework/test/TestPID.java @@ -1,7 +1,7 @@ package com.nutrons.framework.test; -import static com.nutrons.framework.util.FlowOperators.pidLoop; import static com.nutrons.framework.util.FlowOperators.toFlow; +import static com.nutrons.framework.util.FlowOperators.veryBadDontUseEverPidLoop; import io.reactivex.Flowable; import org.junit.Test; @@ -11,7 +11,7 @@ public class TestPID { @Test public void testPIDConcurrency() { Flowable input = toFlow(() -> 1.0); - Flowable output = input.compose(pidLoop(1, 1, 0, 0)); + Flowable output = input.compose(veryBadDontUseEverPidLoop(1, 1, 0, 0)); output.subscribe(x -> { }); // tests that this is done on an io scheduler } From 14288b7966f97dd64ba6680aabd8d19b50094635 Mon Sep 17 00:00:00 2001 From: Brian Estevez Date: Wed, 22 Mar 2017 18:09:07 -0400 Subject: [PATCH 6/6] added PD test case --- test/com/nutrons/framework/test/TestPD.java | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 test/com/nutrons/framework/test/TestPD.java diff --git a/test/com/nutrons/framework/test/TestPD.java b/test/com/nutrons/framework/test/TestPD.java new file mode 100644 index 0000000..656e856 --- /dev/null +++ b/test/com/nutrons/framework/test/TestPD.java @@ -0,0 +1,18 @@ +package com.nutrons.framework.test; + +import static com.nutrons.framework.util.FlowOperators.pdLoop; +import static com.nutrons.framework.util.FlowOperators.toFlow; + +import io.reactivex.Flowable; +import org.junit.Test; + +public class TestPD { + + @Test + public void testPDConcurrency (){ + Flowable input = toFlow(() -> 1.0); + Flowable output = input.compose(pdLoop(1.0, 1.0)); + output.subscribe(x -> { + }); //tests + } +}