diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 250b9d3..90bb80a 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; } @@ -76,33 +76,57 @@ public static T getLastValue(Flowable input) { /** * Creates a PID Loop Function. + * DON'T USE EVER */ - 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; - }; + @Deprecated + public static FlowableTransformer veryBadDontUseEverPidLoop(double proportional, + 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)); } + /** + * 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 + newVal / integralBuffer)); + } + + /** + * RegularPD loop with the 0.0 integral stream. + */ public static FlowableTransformer pdLoop(double proportional, - double derivative) { + 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, + 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); + } }; } 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 + } +} 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 }