From 5548b2dcee75ae32e92926cd69ff95cd501d5db8 Mon Sep 17 00:00:00 2001 From: Josh Young Date: Fri, 17 Feb 2017 08:49:34 -0500 Subject: [PATCH 01/45] Added ID function to flow operatiors --- src/com/nutrons/framework/util/FlowOperators.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 47e3599..2509a1b 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -89,4 +89,9 @@ public static FlowableTransformer pidLoop(double proportional, return output; }; } + public static T printID (T t) { + System.out.println(t); + return t; + } + } From 86931414127e3b0248f583a0885b9a9ad13f3e0d Mon Sep 17 00:00:00 2001 From: Josh Young Date: Fri, 17 Feb 2017 19:42:19 -0500 Subject: [PATCH 02/45] Fixed robot and made disposable flow operator --- src/com/nutrons/framework/Robot.java | 14 +++++++++++--- src/com/nutrons/framework/util/FlowOperators.java | 9 ++++++++- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/com/nutrons/framework/Robot.java b/src/com/nutrons/framework/Robot.java index 3a2e8a1..5890004 100644 --- a/src/com/nutrons/framework/Robot.java +++ b/src/com/nutrons/framework/Robot.java @@ -3,12 +3,14 @@ import static com.nutrons.framework.util.CompMode.AUTO; import static com.nutrons.framework.util.CompMode.TELE; import static com.nutrons.framework.util.CompMode.TEST; +import static io.reactivex.Flowable.combineLatest; import com.nutrons.framework.commands.Command; import com.nutrons.framework.util.CompMode; import com.nutrons.framework.util.FlowOperators; import edu.wpi.first.wpilibj.SampleRobot; import io.reactivex.Flowable; +import io.reactivex.schedulers.Schedulers; public abstract class Robot extends SampleRobot { @@ -72,12 +74,18 @@ public final Flowable competitionStream() { @Override public final void robotMain() { Command auto = this.registerAuto(); - if (auto != null) { - this.competitionStream().filter(x -> x == AUTO).subscribe(x -> auto.terminable(competitionStream().filter(y -> y != AUTO)).execute()); - } this.sm.startCompetition(); + System.out.println("Started Competition"); + combineLatest(enabledStream(), competitionStream(), (x, y) -> x && y.compareTo(AUTO) == 0) + .subscribeOn(Schedulers.io()) + .filter(x -> x) + .map((a) ->{ System.out.println("Starting Autonomous" + auto.getClass().toString()); + return a; + }).subscribe(x -> auto.terminable(competitionStream().filter(y -> y != AUTO)).execute()); + } + @Override public final void startCompetition() { super.startCompetition(); diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index aa6b84f..07b3ba7 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -2,6 +2,8 @@ import io.reactivex.Flowable; import io.reactivex.FlowableTransformer; +import io.reactivex.disposables.CompositeDisposable; +import io.reactivex.disposables.Disposable; import io.reactivex.functions.Function; import io.reactivex.schedulers.Schedulers; @@ -94,5 +96,10 @@ public static T printId(T t) { System.out.println(t); return t; } - + + public static Disposable combineDisposable(Disposable... disposables) { + CompositeDisposable cd = new CompositeDisposable(); + Flowable.fromArray(disposables).blockingSubscribe(cd::add); + return cd; + } } From be674c7d1d3eff51274de9521c85f2f34274947d Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Fri, 17 Feb 2017 20:04:59 -0500 Subject: [PATCH 03/45] initial commit --- .../framework/controllers/LoopSpeedController.java | 4 ++++ src/com/nutrons/framework/controllers/Talon.java | 10 ++++++++++ .../framework/controllers/VirtualSpeedController.java | 10 ++++++++++ 3 files changed, 24 insertions(+) diff --git a/src/com/nutrons/framework/controllers/LoopSpeedController.java b/src/com/nutrons/framework/controllers/LoopSpeedController.java index 449228a..0594b35 100644 --- a/src/com/nutrons/framework/controllers/LoopSpeedController.java +++ b/src/com/nutrons/framework/controllers/LoopSpeedController.java @@ -37,4 +37,8 @@ public void setSetpoint(double setpoint) { public void setReversedSensor(boolean flipped) { this.accept(Events.setReversedSensor(flipped)); } + + public abstract boolean fwdLimitSwitchClosed(); + + public abstract boolean revLimitSwitchClosed(); } diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index fd5de6a..b314a84 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -120,4 +120,14 @@ public double position() { void resetPositionTo(double position) { this.talon.setPosition(position); } + + @Override + public boolean fwdLimitSwitchClosed(){ + return this.talon.isFwdLimitSwitchClosed(); + } + + @Override + public boolean revLimitSwitchClosed(){ + return this.talon.isRevLimitSwitchClosed(); + } } diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index 5a3174d..9389128 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -17,4 +17,14 @@ public void accept(ControllerEvent controllerEvent) { public void setOutputFlipped(boolean flipped) { } + + @Override + public boolean fwdLimitSwitchClosed() { + return false; + } + + @Override + public boolean revLimitSwitchClosed() { + return false; + } } From 828bb9d2c0b921f880d5e833ff2f00a6b2afb10e Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Fri, 17 Feb 2017 23:25:00 -0500 Subject: [PATCH 04/45] working???? --- src/com/nutrons/framework/Robot.java | 1 + src/com/nutrons/framework/StreamManager.java | 7 ++++++- .../controllers/LoopSpeedController.java | 2 ++ .../nutrons/framework/controllers/Talon.java | 21 +++++++++++++++++-- .../controllers/VirtualSpeedController.java | 5 +++++ 5 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/com/nutrons/framework/Robot.java b/src/com/nutrons/framework/Robot.java index 3a2e8a1..06cf4f4 100644 --- a/src/com/nutrons/framework/Robot.java +++ b/src/com/nutrons/framework/Robot.java @@ -75,6 +75,7 @@ public final void robotMain() { if (auto != null) { this.competitionStream().filter(x -> x == AUTO).subscribe(x -> auto.terminable(competitionStream().filter(y -> y != AUTO)).execute()); } + System.out.println("registering subsystems"); this.sm.startCompetition(); } diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index c2ea033..a5bef7f 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -43,7 +43,12 @@ public StreamManager(Flowable enabled, Flowable mode) { * and start the competition loop. */ public final void startCompetition() { - Observable.fromIterable(this.subsystems).blockingSubscribe(Subsystem::registerSubscriptions); + Observable.fromIterable(this.subsystems).blockingSubscribe(x -> { + System.out.println("registering " + x.getClass().getName()); + x.registerSubscriptions(); + System.out.println("registered " + x.getClass().getName()); + }); + System.out.println("all subsystems registered"); this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); Observable.never().blockingSubscribe(); diff --git a/src/com/nutrons/framework/controllers/LoopSpeedController.java b/src/com/nutrons/framework/controllers/LoopSpeedController.java index 449228a..ec05d4b 100644 --- a/src/com/nutrons/framework/controllers/LoopSpeedController.java +++ b/src/com/nutrons/framework/controllers/LoopSpeedController.java @@ -34,6 +34,8 @@ public void setSetpoint(double setpoint) { public abstract void setOutputFlipped(boolean flipped); + public abstract double speed(); + public void setReversedSensor(boolean flipped) { this.accept(Events.setReversedSensor(flipped)); } diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index fd5de6a..f84a7ec 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -42,10 +42,12 @@ public Talon(int port, Talon toFollow) { } void set(double value) { + System.out.println("Received set for value " + value); this.talon.set(value); } void changeControlMode(ControlMode mode) { + System.out.println("Change mode to " + mode.toString()); switch (mode) { case FOLLOWER: this.talon.changeControlMode(CANTalon.TalonControlMode.Follower); @@ -65,6 +67,7 @@ void changeControlMode(ControlMode mode) { } void changeSetpoint(double setpoint) { + System.out.println("Changing setpoint to " + setpoint); this.talon.setSetpoint(setpoint); } @@ -92,15 +95,23 @@ public Flowable feedback() { @Override public void accept(ControllerEvent event) { + System.out.println("Received event " + event.getClass().getName() + " on talon " + this.talon.getDeviceID()); event.actOn(this); } @Override public void setOutputFlipped(boolean flipped) { + System.out.println("output: " + flipped); talon.setInverted(flipped); } + @Override + public double speed() { + return this.talon.getSpeed(); + } + void reverseSensor(boolean flipped) { + System.out.println("sensor: " + flipped); talon.reverseSensor(flipped); } @@ -109,8 +120,10 @@ int id() { } void setOutputVoltage(double min, double max) { - this.talon.configNominalOutputVoltage(Math.max(min, 0.0), Math.min(max, 12.0f)); - this.talon.configPeakOutputVoltage(Math.max(max, 0.0), Math.min(min, 12.0f)); + this.talon.configNominalOutputVoltage(Math.max(min, 0.0), Math.min(max, 0.0)); + this.talon.configPeakOutputVoltage(Math.max(max, 0.0), Math.min(min, 0.0)); + System.out.println("nominal min = " + Math.max(min, 0.0) + " max = " + Math.min(max, 0.0)); + System.out.println("peak min = " + Math.max(max, 0.0) + " max = " + Math.min(min, 0.0)); } public double position() { @@ -120,4 +133,8 @@ public double position() { void resetPositionTo(double position) { this.talon.setPosition(position); } + + public double getClosedLoopError() { + return this.talon.getClosedLoopError(); + } } diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index 5a3174d..fda414d 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -17,4 +17,9 @@ public void accept(ControllerEvent controllerEvent) { public void setOutputFlipped(boolean flipped) { } + + @Override + public double speed() { + return 0; + } } From dcec60250eb4bce205af771906b22ff3ad1a1988 Mon Sep 17 00:00:00 2001 From: Josh Young Date: Sat, 18 Feb 2017 01:00:03 -0500 Subject: [PATCH 05/45] Added Ashers fix to terminate commands --- src/com/nutrons/framework/Robot.java | 11 +------ src/com/nutrons/framework/StreamManager.java | 20 ++++++++++- .../nutrons/framework/commands/Command.java | 33 ++++++++++++------- .../nutrons/framework/util/FlowOperators.java | 17 +++++++++- .../nutrons/framework/test/TestCommand.java | 19 +++++++---- 5 files changed, 70 insertions(+), 30 deletions(-) diff --git a/src/com/nutrons/framework/Robot.java b/src/com/nutrons/framework/Robot.java index 5890004..8d500bd 100644 --- a/src/com/nutrons/framework/Robot.java +++ b/src/com/nutrons/framework/Robot.java @@ -73,16 +73,7 @@ public final Flowable competitionStream() { */ @Override public final void robotMain() { - Command auto = this.registerAuto(); - this.sm.startCompetition(); - System.out.println("Started Competition"); - combineLatest(enabledStream(), competitionStream(), (x, y) -> x && y.compareTo(AUTO) == 0) - .subscribeOn(Schedulers.io()) - .filter(x -> x) - .map((a) ->{ System.out.println("Starting Autonomous" + auto.getClass().toString()); - return a; - }).subscribe(x -> auto.terminable(competitionStream().filter(y -> y != AUTO)).execute()); - + this.sm.startCompetition(() -> this.registerAuto()); } diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index c2ea033..14f0fa1 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -1,10 +1,17 @@ package com.nutrons.framework; +import com.nutrons.framework.commands.Command; import com.nutrons.framework.util.CompMode; import io.reactivex.Flowable; import io.reactivex.Observable; +import io.reactivex.schedulers.Schedulers; + import java.util.ArrayList; import java.util.List; +import java.util.function.Supplier; + +import static com.nutrons.framework.util.CompMode.AUTO; +import static io.reactivex.Flowable.combineLatest; /** * This class sets up the I/O factories and initializes subsystems. @@ -42,8 +49,19 @@ public StreamManager(Flowable enabled, Flowable mode) { * Called to by the bootstrapper to subscribe subsystem streams, * and start the competition loop. */ - public final void startCompetition() { + public final void startCompetition(Supplier autoSupplier) { Observable.fromIterable(this.subsystems).blockingSubscribe(Subsystem::registerSubscriptions); + Command auto = autoSupplier.get(); + if (auto != null) { + combineLatest(this.enabled, this.mode, (x, y) -> x && y.compareTo(AUTO) == 0) + .subscribeOn(Schedulers.io()) + .filter(x -> x) + .map((a) -> { + System.out.println("Starting Autonomous" + auto.getClass().toString()); + return a; + }).subscribe(x -> auto.terminable(mode.filter(y -> y != AUTO) + .mergeWith(enabled.filter(z -> z).map(z -> AUTO))).execute()); + } this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); Observable.never().blockingSubscribe(); diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 7865f27..00086e0 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -1,5 +1,6 @@ package com.nutrons.framework.commands; +import com.nutrons.framework.util.FlowOperators; import io.reactivex.Flowable; import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; @@ -101,9 +102,13 @@ public Command when(Supplier startCondition) { */ public Command terminable(Publisher terminator) { return new Command(() -> { - Flowable terminatorFlowable = this.execute(); + Flowable terminatorFlowable = this.execute().replay(); return Flowable.defer(() -> Flowable.never().takeUntil(terminator) - .mergeWith(Flowable.just(() -> terminatorFlowable.subscribe(Terminator::run)))); + .mergeWith(Flowable.just(() -> { + System.out.println("doing the terminates"); + terminatorFlowable.subscribeOn(Schedulers.io()).map(FlowOperators::printId).subscribe(Terminator::run); + System.out.println("terminates terminating"); + }))); }); } @@ -131,22 +136,28 @@ public Command delayStart(long delay, TimeUnit unit) { return this.startable(Flowable.timer(delay, unit)); } - /** - * Copies this command into one which will delay its completion until a certain time has passed. - */ - public Command delayTermination(long delay, TimeUnit unit) { - return parallel(this, new Command(Flowable::never).terminable(Flowable.timer(delay, unit))); + public Command killAfter(long delay, TimeUnit unit) { + return this.terminable(Flowable.timer(delay, unit)).terminateOnFinish(); } - public Command killAfter(long delay, TimeUnit unit) { - return this.terminable(Flowable.timer(delay, unit)); + private Command terminateOnFinish() { + return new Command(() -> { + Flowable terms = this.execute(); + System.out.println("stuff"); + Flowable cached = terms.repeat(); + System.out.println("stuff2"); + return terms.doOnComplete(() -> { + System.out.println("compltd"); + cached.subscribe(x -> x.run()); + }); + }); } @Override public Flowable execute() { Flowable terms = source.execute(); - terms.subscribeOn(Schedulers.io()).toList() - .subscribe(t -> Flowable.fromIterable(t).subscribe(Terminator::run)); + /*terms.subscribeOn(Schedulers.io()).toList() + .subscribe(t -> Flowable.fromIterable(t).subscribe(Terminator::run)); */ return terms; } } diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index cbbf060..dcffd43 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -7,6 +7,7 @@ import io.reactivex.functions.Function; import io.reactivex.schedulers.Schedulers; +import java.awt.*; import java.util.concurrent.TimeUnit; import java.util.function.Supplier; @@ -104,7 +105,21 @@ public static T printId(T t) { public static Disposable combineDisposable(Disposable... disposables) { CompositeDisposable cd = new CompositeDisposable(); + CompositeDisposable dvd = new CompositeDisposable(); + dvd.add(new Disposable() { + @Override + public void dispose() { + System.out.println("disposing"); + cd.dispose(); + System.out.println("done disposing"); + } + + @Override + public boolean isDisposed() { + return cd.isDisposed(); + } + }); Flowable.fromArray(disposables).blockingSubscribe(cd::add); - return cd; + return dvd; } } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index 449ca30..f9aecea 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -4,6 +4,7 @@ import com.nutrons.framework.commands.Terminator; import com.nutrons.framework.commands.TerminatorWrapper; import io.reactivex.Flowable; +import io.reactivex.functions.Action; import io.reactivex.processors.PublishProcessor; import io.reactivex.schedulers.Schedulers; import org.junit.Before; @@ -22,7 +23,7 @@ public class TestCommand { public void setupCommands() { delay = Command.fromAction(() -> { - }).delayTermination(1000, TimeUnit.MILLISECONDS); + }).killAfter(1000, TimeUnit.MILLISECONDS); } @Test @@ -125,11 +126,15 @@ static void waitForCommand(Flowable commandExecution) { public void killAfter() throws InterruptedException { int[] record = new int[1]; long start = System.currentTimeMillis(); - Command.just(() -> Flowable.just(() -> { - assertTrue(System.currentTimeMillis() - 2000 < start); - record[0] = 1; - })).delayTermination(1000, TimeUnit.SECONDS).killAfter(1, TimeUnit.SECONDS).execute(); - Thread.sleep(2000); + Command.just(() -> { + System.out.println("executed"); + return Flowable.just(() -> { + System.out.println("asdf"); + record[0] = 1; + assertTrue(System.currentTimeMillis() - 2000 < start); + }).doOnComplete(() -> System.out.println("hi")); + }).killAfter(2, TimeUnit.SECONDS).execute().subscribe(Terminator::run); + Thread.sleep(4000); assertTrue(record[0] == 1); } @@ -148,7 +153,7 @@ public void testSwitch() throws InterruptedException { })); }); Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) - .execute().blockingSubscribe(); + .execute().blockingSubscribe(Terminator::run); Thread.sleep(2000); assertTrue(record[0] == 0); } From 7b43c5223833b13c29dbd6ae4b12df2003276a97 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Sat, 18 Feb 2017 08:26:16 -0500 Subject: [PATCH 06/45] command update --- .../nutrons/framework/commands/Command.java | 20 +++++-- .../commands/FlattenedTerminator.java | 54 +++++++++++++++++++ .../framework/commands/ParallelCommand.java | 44 ++------------- .../framework/commands/SerialCommand.java | 43 ++------------- .../nutrons/framework/test/TestCommand.java | 15 +++--- 5 files changed, 84 insertions(+), 92 deletions(-) create mode 100644 src/com/nutrons/framework/commands/FlattenedTerminator.java diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 7865f27..3633d6c 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -102,8 +102,9 @@ public Command when(Supplier startCondition) { public Command terminable(Publisher terminator) { return new Command(() -> { Flowable terminatorFlowable = this.execute(); + Terminator multi = FlattenedTerminator.from(terminatorFlowable); return Flowable.defer(() -> Flowable.never().takeUntil(terminator) - .mergeWith(Flowable.just(() -> terminatorFlowable.subscribe(Terminator::run)))); + .mergeWith(Flowable.just(multi::run))); }); } @@ -139,14 +140,25 @@ public Command delayTermination(long delay, TimeUnit unit) { } public Command killAfter(long delay, TimeUnit unit) { - return this.terminable(Flowable.timer(delay, unit)); + return Command.just(() -> { + Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(); + return terms.doOnComplete(() -> { + System.out.println("completed"); + FlattenedTerminator.from(terms).toSingle() + .subscribe(Terminator::run); + }); + }); } @Override public Flowable execute() { Flowable terms = source.execute(); - terms.subscribeOn(Schedulers.io()).toList() - .subscribe(t -> Flowable.fromIterable(t).subscribe(Terminator::run)); + return terms; + } + + public Flowable startExecution() { + Flowable terms = this.execute().subscribeOn(Schedulers.io()); + terms.subscribe(); return terms; } } diff --git a/src/com/nutrons/framework/commands/FlattenedTerminator.java b/src/com/nutrons/framework/commands/FlattenedTerminator.java new file mode 100644 index 0000000..33779b4 --- /dev/null +++ b/src/com/nutrons/framework/commands/FlattenedTerminator.java @@ -0,0 +1,54 @@ +package com.nutrons.framework.commands; + +import io.reactivex.Flowable; +import io.reactivex.schedulers.Schedulers; + +import java.util.ArrayList; +import java.util.concurrent.atomic.AtomicBoolean; + +class FlattenedTerminator implements Terminator { + + private final AtomicBoolean lock; + private final ArrayList terminators; + private final Flowable terminatorStream; + + private FlattenedTerminator(Flowable terminators) { + this.lock = new AtomicBoolean(false); + this.terminators = new ArrayList<>(); + this.terminatorStream = terminators; + terminators.subscribeOn(Schedulers.io()).subscribe(x -> { + if (!lock.get()) { + synchronized (lock) { + if (!lock.get()) { + this.terminators.add(x); + return; + } + } + } + x.run(); + }); + } + + static FlattenedTerminator from(Flowable terminators) { + return new FlattenedTerminator(terminators); + } + + public Flowable toSingle() { + return Flowable.just(this).mergeWith(terminatorStream.ignoreElements().toFlowable()) + .subscribeOn(Schedulers.io()); + } + + @Override + public void run() { + if (!lock.get()) { + synchronized (lock) { + if (!lock.get()) { + lock.set(true); + for (Terminator terminator : terminators) { + terminator.run(); + } + } + } + } + } +} diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index 3e6accc..5e73ccb 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -3,10 +3,6 @@ import io.reactivex.Flowable; import io.reactivex.schedulers.Schedulers; -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.atomic.AtomicBoolean; - public class ParallelCommand implements CommandWorkUnit { private final Flowable commands; @@ -17,44 +13,10 @@ public class ParallelCommand implements CommandWorkUnit { @Override public Flowable execute() { - final AtomicBoolean lock = new AtomicBoolean(false); - final List terminators = new ArrayList<>(); - Flowable terminatorFlow = this.commands.flatMap(x -> x.execute().subscribeOn(Schedulers.io())) + Flowable terminators = this.commands.flatMap(x -> x.execute().subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); - ; - terminatorFlow.subscribe(x -> { - if (!lock.get()) { - synchronized (lock) { - if (!lock.get()) { - terminators.add(x); - return; - } - } - } - x.run(); - }); - return Flowable.just( - new TerminatorWrapper(new ParallelTerminator(lock, terminators))) - .mergeWith(terminatorFlow.ignoreElements().toFlowable()); - } - - private class ParallelTerminator implements Runnable { - private final AtomicBoolean lock; - private final List terminators; - - private ParallelTerminator(AtomicBoolean lock, List terminators) { - this.lock = lock; - this.terminators = terminators; - } - @Override - public void run() { - synchronized (lock) { - lock.set(true); - for (Terminator terminator : terminators) { - terminator.run(); - } - } - } + return Flowable.just(FlattenedTerminator.from(terminators)) + .mergeWith(terminators.ignoreElements().toFlowable()); } } diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index 38b6ddf..a55266b 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -3,10 +3,6 @@ import io.reactivex.Flowable; import io.reactivex.schedulers.Schedulers; -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.atomic.AtomicBoolean; - public class SerialCommand implements CommandWorkUnit { private final Flowable commands; @@ -17,43 +13,10 @@ public class SerialCommand implements CommandWorkUnit { @Override public Flowable execute() { - final AtomicBoolean lock = new AtomicBoolean(false); - final List terminators = new ArrayList<>(); - Flowable terminatorFlow = this.commands.concatMap(x -> x.execute().subscribeOn(Schedulers.io())) + Flowable terminators = this.commands.concatMap(x -> x.execute().subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); - terminatorFlow.subscribe(x -> { - if (!lock.get()) { - synchronized (lock) { - if (!lock.get()) { - terminators.add(x); - return; - } - } - } - x.run(); - }); - return Flowable.just( - new TerminatorWrapper(new SerialTerminator(lock, terminators))) - .mergeWith(terminatorFlow.ignoreElements().toFlowable()); - } - - private class SerialTerminator implements Runnable { - private final AtomicBoolean lock; - private final List terminators; - - private SerialTerminator(AtomicBoolean lock, List terminators) { - this.lock = lock; - this.terminators = terminators; - } - @Override - public void run() { - synchronized (lock) { - lock.set(true); - for (Terminator terminator : terminators) { - terminator.run(); - } - } - } + return Flowable.just(FlattenedTerminator.from(terminators)) + .mergeWith(terminators.ignoreElements().toFlowable()); } } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index 449ca30..1fa9006 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -18,6 +18,10 @@ public class TestCommand { private Command delay; + static void waitForCommand(Flowable commandExecution) { + commandExecution.blockingSubscribe(); + } + @Before public void setupCommands() { delay = Command.fromAction(() -> { @@ -117,19 +121,16 @@ public void parallelAndSerial() { assertTrue(System.currentTimeMillis() - 3000 < start); } - static void waitForCommand(Flowable commandExecution) { - commandExecution.blockingSubscribe(); - } - @Test public void killAfter() throws InterruptedException { int[] record = new int[1]; long start = System.currentTimeMillis(); Command.just(() -> Flowable.just(() -> { + System.out.println("hi"); assertTrue(System.currentTimeMillis() - 2000 < start); record[0] = 1; - })).delayTermination(1000, TimeUnit.SECONDS).killAfter(1, TimeUnit.SECONDS).execute(); - Thread.sleep(2000); + })).delayTermination(1000, TimeUnit.SECONDS).killAfter(1, TimeUnit.SECONDS).startExecution(); + Thread.sleep(4000); assertTrue(record[0] == 1); } @@ -148,7 +149,7 @@ public void testSwitch() throws InterruptedException { })); }); Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) - .execute().blockingSubscribe(); + .execute().blockingSubscribe(Terminator::run); Thread.sleep(2000); assertTrue(record[0] == 0); } From 60f2ce43067518c1cc765e3d02853f0ff00d6890 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Sat, 18 Feb 2017 09:48:05 -0500 Subject: [PATCH 07/45] updated --- src/com/nutrons/framework/commands/Command.java | 4 ---- test/com/nutrons/framework/test/TestCommand.java | 2 +- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 4ce0cbb..42c8ac7 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -132,10 +132,6 @@ public Command delayStart(long delay, TimeUnit unit) { return this.startable(Flowable.timer(delay, unit)); } - public Command delayTermination(long delay, TimeUnit unit) { - return parallel(this, new Command(Flowable::never).terminable(Flowable.timer(delay, unit))); - } - public Command killAfter(long delay, TimeUnit unit) { return Command.just(() -> { Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(); diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index 57a206e..18ff929 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -129,7 +129,7 @@ public void killAfter() throws InterruptedException { System.out.println("hi"); assertTrue(System.currentTimeMillis() - 2000 < start); record[0] = 1; - })).delayTermination(1000, TimeUnit.SECONDS).killAfter(1, TimeUnit.SECONDS).startExecution(); + })).killAfter(1, TimeUnit.SECONDS).startExecution(); Thread.sleep(4000); assertTrue(record[0] == 1); } From 6eea67fc6b25b384dc45e641474ce1699c4d04b5 Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Sat, 18 Feb 2017 11:20:17 -0500 Subject: [PATCH 08/45] auto --- src/com/nutrons/framework/StreamManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index d57ef79..8b2735f 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -65,7 +65,7 @@ public final void startCompetition(Supplier autoSupplier) { System.out.println("Starting Autonomous" + auto.getClass().toString()); return a; }).subscribe(x -> auto.terminable(mode.filter(y -> y != AUTO) - .mergeWith(enabled.filter(z -> z).map(z -> AUTO))).execute()); + .mergeWith(enabled.filter(z -> z).map(z -> AUTO))).startExecution()); } this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); From 06cbd85544192e291dec6293db55da47df62c9d2 Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Sun, 19 Feb 2017 13:08:55 -0500 Subject: [PATCH 09/45] added registerTele command --- src/com/nutrons/framework/Robot.java | 6 +++++- src/com/nutrons/framework/StreamManager.java | 16 ++++++++++++++-- src/com/nutrons/framework/controllers/Talon.java | 4 ++++ 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/com/nutrons/framework/Robot.java b/src/com/nutrons/framework/Robot.java index 8d500bd..6423377 100644 --- a/src/com/nutrons/framework/Robot.java +++ b/src/com/nutrons/framework/Robot.java @@ -73,7 +73,7 @@ public final Flowable competitionStream() { */ @Override public final void robotMain() { - this.sm.startCompetition(() -> this.registerAuto()); + this.sm.startCompetition(this::registerAuto, this::registerTele); } @@ -90,6 +90,10 @@ protected Command registerAuto() { return null; } + protected Command registerTele() { + return null; + } + @Override public final void autonomous() { } diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index 8b2735f..7ccaa52 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -11,6 +11,7 @@ import java.util.function.Supplier; import static com.nutrons.framework.util.CompMode.AUTO; +import static com.nutrons.framework.util.CompMode.TELE; import static io.reactivex.Flowable.combineLatest; /** @@ -49,7 +50,7 @@ public StreamManager(Flowable enabled, Flowable mode) { * Called to by the bootstrapper to subscribe subsystem streams, * and start the competition loop. */ - public final void startCompetition(Supplier autoSupplier) { + public final void startCompetition(Supplier autoSupplier, Supplier teleopSupplier) { Observable.fromIterable(this.subsystems).blockingSubscribe(x -> { System.out.println("registering " + x.getClass().getName()); x.registerSubscriptions(); @@ -62,11 +63,22 @@ public final void startCompetition(Supplier autoSupplier) { .subscribeOn(Schedulers.io()) .filter(x -> x) .map((a) -> { - System.out.println("Starting Autonomous" + auto.getClass().toString()); + System.out.println("Starting Autonomous: " + auto.getClass().toString()); return a; }).subscribe(x -> auto.terminable(mode.filter(y -> y != AUTO) .mergeWith(enabled.filter(z -> z).map(z -> AUTO))).startExecution()); } + Command tele = teleopSupplier.get(); + if (tele != null) { + combineLatest(this.enabled, this.mode, (x, y) -> x && y.compareTo(TELE) == 0) + .subscribeOn(Schedulers.io()) + .filter(x -> x) + .map((a) -> { + System.out.println("Starting Teleop: " + tele.getClass().toString()); + return a; + }).subscribe(x -> tele.terminable(mode.filter(y -> y != TELE) + .mergeWith(enabled.filter(z -> z).map(z -> TELE))).startExecution()); + } this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); Observable.never().blockingSubscribe(); diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index aedfaab..9d99657 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -110,6 +110,10 @@ public double speed() { return this.talon.getSpeed(); } + public void setFeedbackDevice(CANTalon.FeedbackDevice device) { + this.talon.setFeedbackDevice(device); + } + void reverseSensor(boolean flipped) { System.out.println("sensor: " + flipped); talon.reverseSensor(flipped); From c1dcb2b9d8bf61b7ce952fe318c9931f7f636e22 Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Sun, 19 Feb 2017 18:23:06 -0500 Subject: [PATCH 10/45] had to make toFlowBackpressure backpressureless, and other minor tune ups --- src/com/nutrons/framework/StreamManager.java | 4 ++-- .../nutrons/framework/commands/Command.java | 22 +++++++++++++------ .../controllers/LoopSpeedController.java | 2 ++ .../nutrons/framework/controllers/Talon.java | 7 +----- .../controllers/VirtualSpeedController.java | 5 +++++ .../nutrons/framework/util/FlowOperators.java | 20 +++-------------- 6 files changed, 28 insertions(+), 32 deletions(-) diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index 7ccaa52..0776213 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -66,7 +66,7 @@ public final void startCompetition(Supplier autoSupplier, Supplier auto.terminable(mode.filter(y -> y != AUTO) - .mergeWith(enabled.filter(z -> z).map(z -> AUTO))).startExecution()); + .mergeWith(enabled.filter(z -> !z).map(z -> AUTO))).startExecution()); } Command tele = teleopSupplier.get(); if (tele != null) { @@ -77,7 +77,7 @@ public final void startCompetition(Supplier autoSupplier, Supplier tele.terminable(mode.filter(y -> y != TELE) - .mergeWith(enabled.filter(z -> z).map(z -> TELE))).startExecution()); + .mergeWith(enabled.filter(z -> !z).map(z -> TELE))).startExecution()); } this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 42c8ac7..b13656c 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -1,6 +1,8 @@ package com.nutrons.framework.commands; +import com.nutrons.framework.util.FlowOperators; import io.reactivex.Flowable; +import io.reactivex.Observable; import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.schedulers.Schedulers; @@ -9,6 +11,7 @@ import java.util.concurrent.TimeUnit; import java.util.function.Supplier; +import static com.nutrons.framework.util.FlowOperators.printId; import static com.nutrons.framework.util.FlowOperators.toFlow; public class Command implements CommandWorkUnit { @@ -79,6 +82,10 @@ public static Command fromSwitch(Publisher commandStr })); } + public Command addFinalTerminator(Terminator terminator) { + return Command.just(() -> this.source.execute().flatMap(x -> Flowable.just(x, terminator))); + } + /** * Copies this command into one which will not execute until 'starter' emits an item. */ @@ -101,9 +108,11 @@ public Command when(Supplier startCondition) { */ public Command terminable(Publisher terminator) { return new Command(() -> { - Flowable terminatorFlowable = this.execute(); - Terminator multi = FlattenedTerminator.from(terminatorFlowable); - return Flowable.defer(() -> Flowable.never().takeUntil(terminator) + ConnectableFlowable flowterminator = Flowable.fromPublisher(terminator).publish(); + flowterminator.connect(); + Flowable sourceTerminator = this.execute(); + Terminator multi = FlattenedTerminator.from(sourceTerminator); + return Flowable.defer(() -> Flowable.never().takeUntil(flowterminator) .mergeWith(Flowable.just(multi::run))); }); } @@ -113,9 +122,8 @@ public Command terminable(Publisher terminator) { * will only complete once endCondition returns true. */ public Command until(Supplier endCondition) { - Flowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x).onBackpressureDrop(); - return new Command(() -> - Flowable.never().mergeWith(this.execute())).terminable(terminator); + Flowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x).onBackpressureDrop().publish(); + return new Command(this::execute).terminable(terminator); } /** @@ -150,7 +158,7 @@ public Flowable execute() { public Flowable startExecution() { Flowable terms = this.execute().subscribeOn(Schedulers.io()); - terms.subscribe(); + terms.toList().subscribe(x -> Observable.fromIterable(x).blockingSubscribe(Terminator::run)); return terms; } } diff --git a/src/com/nutrons/framework/controllers/LoopSpeedController.java b/src/com/nutrons/framework/controllers/LoopSpeedController.java index a8cba87..38ad5ab 100644 --- a/src/com/nutrons/framework/controllers/LoopSpeedController.java +++ b/src/com/nutrons/framework/controllers/LoopSpeedController.java @@ -43,4 +43,6 @@ public void setReversedSensor(boolean flipped) { public abstract boolean fwdLimitSwitchClosed(); public abstract boolean revLimitSwitchClosed(); + + public abstract double position(); } diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index 9d99657..4235f07 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -42,12 +42,10 @@ public Talon(int port, Talon toFollow) { } void set(double value) { - System.out.println("Received set for value " + value); this.talon.set(value); } void changeControlMode(ControlMode mode) { - System.out.println("Change mode to " + mode.toString()); switch (mode) { case FOLLOWER: this.talon.changeControlMode(CANTalon.TalonControlMode.Follower); @@ -67,7 +65,6 @@ void changeControlMode(ControlMode mode) { } void changeSetpoint(double setpoint) { - System.out.println("Changing setpoint to " + setpoint); this.talon.setSetpoint(setpoint); } @@ -95,13 +92,11 @@ public Flowable feedback() { @Override public void accept(ControllerEvent event) { - System.out.println("Received event " + event.getClass().getName() + " on talon " + this.talon.getDeviceID()); event.actOn(this); } @Override public void setOutputFlipped(boolean flipped) { - System.out.println("output: " + flipped); talon.setInverted(flipped); } @@ -115,7 +110,6 @@ public void setFeedbackDevice(CANTalon.FeedbackDevice device) { } void reverseSensor(boolean flipped) { - System.out.println("sensor: " + flipped); talon.reverseSensor(flipped); } @@ -130,6 +124,7 @@ void setOutputVoltage(double min, double max) { System.out.println("peak min = " + Math.max(max, 0.0) + " max = " + Math.min(min, 0.0)); } + @Override public double position() { return this.talon.getPosition(); } diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index 363d973..0663288 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -28,6 +28,11 @@ public boolean revLimitSwitchClosed() { return false; } + @Override + public double position() { + return 0; + } + public double speed() { return 0; } diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index dcffd43..ea421e2 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -32,8 +32,8 @@ public static Flowable toFlow(Supplier supplier, long ignored, TimeUni * @param the type of the Flowable and Supplier */ public static Flowable toFlowBackpressure(Supplier supplier, long ignored, TimeUnit unit) { - return Flowable.interval(ignored, unit).subscribeOn(Schedulers.io()) - .map(x -> supplier.get()).observeOn(Schedulers.computation()); + return Flowable.interval(ignored, unit).onBackpressureDrop().subscribeOn(Schedulers.io()) + .map(x -> supplier.get()).onBackpressureDrop().observeOn(Schedulers.computation()); } /** @@ -105,21 +105,7 @@ public static T printId(T t) { public static Disposable combineDisposable(Disposable... disposables) { CompositeDisposable cd = new CompositeDisposable(); - CompositeDisposable dvd = new CompositeDisposable(); - dvd.add(new Disposable() { - @Override - public void dispose() { - System.out.println("disposing"); - cd.dispose(); - System.out.println("done disposing"); - } - - @Override - public boolean isDisposed() { - return cd.isDisposed(); - } - }); Flowable.fromArray(disposables).blockingSubscribe(cd::add); - return dvd; + return cd; } } From e13b5293ae5634f164fd8a9b76373a0166918248 Mon Sep 17 00:00:00 2001 From: Josh Young Date: Sun, 19 Feb 2017 21:00:16 -0500 Subject: [PATCH 11/45] Made RevServo controller events and functionality... --- .../controllers/ControllerEvent.java | 1 + .../nutrons/framework/controllers/Events.java | 8 +++++ .../framework/controllers/RevServo.java | 26 +++++++++++++++ .../controllers/ServoController.java | 13 ++++++++ .../framework/controllers/ServoEvent.java | 10 ++++++ .../framework/controllers/SetAngleEvent.java | 31 +++++++++++++++++ .../framework/controllers/SetEvent.java | 33 +++++++++++++++++++ 7 files changed, 122 insertions(+) create mode 100644 src/com/nutrons/framework/controllers/RevServo.java create mode 100644 src/com/nutrons/framework/controllers/ServoController.java create mode 100644 src/com/nutrons/framework/controllers/ServoEvent.java create mode 100644 src/com/nutrons/framework/controllers/SetAngleEvent.java create mode 100644 src/com/nutrons/framework/controllers/SetEvent.java diff --git a/src/com/nutrons/framework/controllers/ControllerEvent.java b/src/com/nutrons/framework/controllers/ControllerEvent.java index ccaf9af..5ac1a94 100644 --- a/src/com/nutrons/framework/controllers/ControllerEvent.java +++ b/src/com/nutrons/framework/controllers/ControllerEvent.java @@ -11,4 +11,5 @@ default void actOn(VirtualSpeedController controller) { throw new EventUnimplementedException( controller.getClass().toString(), this.getClass().toString()); } + } diff --git a/src/com/nutrons/framework/controllers/Events.java b/src/com/nutrons/framework/controllers/Events.java index 6f03e43..d9b2e05 100644 --- a/src/com/nutrons/framework/controllers/Events.java +++ b/src/com/nutrons/framework/controllers/Events.java @@ -88,4 +88,12 @@ public static ControllerEvent setOutputVoltage(double min, double max) { public static ControllerEvent resetPosition(double position) { return new ResetPositionEvent(position); } + + public static ServoEvent setAngle(double angle) { + return new SetAngleEvent(angle); + } + + public static ServoEvent set(double value) { + return new SetEvent(value); + } } diff --git a/src/com/nutrons/framework/controllers/RevServo.java b/src/com/nutrons/framework/controllers/RevServo.java new file mode 100644 index 0000000..bcc15e0 --- /dev/null +++ b/src/com/nutrons/framework/controllers/RevServo.java @@ -0,0 +1,26 @@ +package com.nutrons.framework.controllers; + +import edu.wpi.first.wpilibj.Servo; + +public class RevServo extends ServoController { + + private final Servo servo; + + public RevServo(int port) { + servo = new Servo(port); + } + + @Override + public void accept(ServoEvent servoEvent) { + servoEvent.actOn(this); + } + + public void setAngle(double angle) { + this.servo.setAngle(angle); + } + + public void set(double value) { + this.servo.set(value); + } + +} diff --git a/src/com/nutrons/framework/controllers/ServoController.java b/src/com/nutrons/framework/controllers/ServoController.java new file mode 100644 index 0000000..2d32876 --- /dev/null +++ b/src/com/nutrons/framework/controllers/ServoController.java @@ -0,0 +1,13 @@ +package com.nutrons.framework.controllers; + +import io.reactivex.functions.Consumer; + +public abstract class ServoController implements Consumer { + + @Override + public abstract void accept(ServoEvent servoEvent); + + public void setAngle(double angle) { this.accept(Events.setAngle(angle)); } + + public void set(double value) { this.accept(Events.set(value)); } +} diff --git a/src/com/nutrons/framework/controllers/ServoEvent.java b/src/com/nutrons/framework/controllers/ServoEvent.java new file mode 100644 index 0000000..77617f5 --- /dev/null +++ b/src/com/nutrons/framework/controllers/ServoEvent.java @@ -0,0 +1,10 @@ +package com.nutrons.framework.controllers; + +public interface ServoEvent { + + default void actOn(RevServo servo) { + throw new EventUnimplementedException( + servo.getClass().toString(), this.getClass().toString()); + } + +} diff --git a/src/com/nutrons/framework/controllers/SetAngleEvent.java b/src/com/nutrons/framework/controllers/SetAngleEvent.java new file mode 100644 index 0000000..3e01f24 --- /dev/null +++ b/src/com/nutrons/framework/controllers/SetAngleEvent.java @@ -0,0 +1,31 @@ +package com.nutrons.framework.controllers; + +public class SetAngleEvent implements ServoEvent { + + private final double angle; + + /** + * An event which sets the servo to turn to a certain angle. + * + * @param angle the angle to turn the servo to -90 (left) to 90 (right). + */ + public SetAngleEvent(double angle) { + this.angle = angle; + } + + public double angle() { + return this.angle; + + } + + @Override + public void actOn(RevServo servo) { + if (Math.abs(angle) > 90.0) { + throw new EventUnimplementedException( + "Angle greater than 90 degrees is not supported for Servos"); + } + servo.setAngle(angle); + + } + +} diff --git a/src/com/nutrons/framework/controllers/SetEvent.java b/src/com/nutrons/framework/controllers/SetEvent.java new file mode 100644 index 0000000..8cb8f5f --- /dev/null +++ b/src/com/nutrons/framework/controllers/SetEvent.java @@ -0,0 +1,33 @@ +package com.nutrons.framework.controllers; + +public class SetEvent implements ServoEvent { + + private final double value; + + /** + * An event which sets the servo to turn to a certain angle. + * + * @param value the input value to give the servo -1 is full left 1 is full right. + */ + public SetEvent(double value) { + this.value = value; + + } + + public double value() { + return this.value; + + } + + @Override + public void actOn(RevServo servo) { + if (Math.abs(value) > 1.0) { + throw new EventUnimplementedException( + "Value input greater than 1.0 is not supported for this servo"); + + } + servo.set(value); + + } + +} From e23bc5e114317b4d04ac8d9784f86e01a48c6bf6 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Mon, 20 Feb 2017 10:31:31 -0500 Subject: [PATCH 12/45] edited speed of serial --- src/com/nutrons/framework/controllers/Talon.java | 9 ++++----- src/com/nutrons/framework/inputs/Serial.java | 3 ++- src/com/nutrons/framework/util/FlowOperators.java | 4 ++++ 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index aedfaab..26acf27 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -42,12 +42,10 @@ public Talon(int port, Talon toFollow) { } void set(double value) { - System.out.println("Received set for value " + value); this.talon.set(value); } void changeControlMode(ControlMode mode) { - System.out.println("Change mode to " + mode.toString()); switch (mode) { case FOLLOWER: this.talon.changeControlMode(CANTalon.TalonControlMode.Follower); @@ -95,7 +93,6 @@ public Flowable feedback() { @Override public void accept(ControllerEvent event) { - System.out.println("Received event " + event.getClass().getName() + " on talon " + this.talon.getDeviceID()); event.actOn(this); } @@ -122,8 +119,6 @@ int id() { void setOutputVoltage(double min, double max) { this.talon.configNominalOutputVoltage(Math.max(min, 0.0), Math.min(max, 0.0)); this.talon.configPeakOutputVoltage(Math.max(max, 0.0), Math.min(min, 0.0)); - System.out.println("nominal min = " + Math.max(min, 0.0) + " max = " + Math.min(max, 0.0)); - System.out.println("peak min = " + Math.max(max, 0.0) + " max = " + Math.min(min, 0.0)); } public double position() { @@ -148,4 +143,8 @@ public boolean revLimitSwitchClosed() { public double getClosedLoopError() { return this.talon.getClosedLoopError(); } + + public void reverseOutput(boolean flip){ + this.talon.reverseOutput(flip); + } } diff --git a/src/com/nutrons/framework/inputs/Serial.java b/src/com/nutrons/framework/inputs/Serial.java index b3751ed..3639ec3 100644 --- a/src/com/nutrons/framework/inputs/Serial.java +++ b/src/com/nutrons/framework/inputs/Serial.java @@ -9,6 +9,7 @@ import java.util.function.Supplier; import static com.nutrons.framework.util.FlowOperators.toFlow; +import static com.nutrons.framework.util.FlowOperators.toFlowFast; /** * A wrapper around WPI's SerialPort class which provides @@ -81,7 +82,7 @@ public Serial(int baudrate, } return serial.read(packetLength); }; - this.dataStream = toFlow(new IntervalCache(100, supplier)) + this.dataStream = toFlowFast(new IntervalCache(50, supplier)) .filter(x -> x.length == packetLength); } catch (RuntimeException e) { System.out.println(e.getMessage()); diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index f7a117e..99ba114 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -42,6 +42,10 @@ public static Flowable toFlow(Supplier supplier) { return toFlow(supplier, 100, TimeUnit.MILLISECONDS); } + public static Flowable toFlowFast(Supplier supplier) { + return toFlow(supplier, 50, TimeUnit.MILLISECONDS); + } + /** * Modifies motor input to prevent motors running at very small values * From 62d8d02ac504f29a46e68cbf13a6aa25aa396960 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Mon, 20 Feb 2017 11:04:34 -0500 Subject: [PATCH 13/45] some command changes... hope this still works --- src/com/nutrons/framework/StreamManager.java | 4 +- .../nutrons/framework/commands/Command.java | 50 ++++++++----------- .../framework/commands/CommandWorkUnit.java | 2 +- .../framework/commands/ParallelCommand.java | 11 ++-- .../framework/commands/SerialCommand.java | 11 ++-- .../nutrons/framework/test/TestCommand.java | 31 ++++++------ .../framework/test/TestCommandReuse.java | 8 +-- 7 files changed, 55 insertions(+), 62 deletions(-) diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index 0776213..54f72fd 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -66,7 +66,7 @@ public final void startCompetition(Supplier autoSupplier, Supplier auto.terminable(mode.filter(y -> y != AUTO) - .mergeWith(enabled.filter(z -> !z).map(z -> AUTO))).startExecution()); + .mergeWith(enabled.filter(z -> !z).map(z -> AUTO))).execute(true)); } Command tele = teleopSupplier.get(); if (tele != null) { @@ -77,7 +77,7 @@ public final void startCompetition(Supplier autoSupplier, Supplier tele.terminable(mode.filter(y -> y != TELE) - .mergeWith(enabled.filter(z -> !z).map(z -> TELE))).startExecution()); + .mergeWith(enabled.filter(z -> !z).map(z -> TELE))).execute(true)); } this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index b13656c..dccaeb6 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -1,6 +1,5 @@ package com.nutrons.framework.commands; -import com.nutrons.framework.util.FlowOperators; import io.reactivex.Flowable; import io.reactivex.Observable; import io.reactivex.disposables.Disposable; @@ -11,14 +10,13 @@ import java.util.concurrent.TimeUnit; import java.util.function.Supplier; -import static com.nutrons.framework.util.FlowOperators.printId; import static com.nutrons.framework.util.FlowOperators.toFlow; public class Command implements CommandWorkUnit { // emptyPulse sends items on an interval, and is used in the 'until' and 'when' methods // to determine how often to test the predicates. private static final ConnectableFlowable emptyPulse = - toFlow(() -> (CommandWorkUnit) () -> Flowable.just(() -> { + toFlow(() -> (CommandWorkUnit) x -> Flowable.just(() -> { })).subscribeOn(Schedulers.io()).onBackpressureDrop().publish(); private final CommandWorkUnit source; @@ -31,7 +29,7 @@ private Command(CommandWorkUnit command) { * Create a command that executes the given action. */ public static Command fromAction(Runnable action) { - return new Command(() -> { + return Command.just(x -> { action.run(); return Flowable.just(() -> { }); @@ -43,7 +41,7 @@ public static Command fromAction(Runnable action) { * such as a function which subscribes a stream to a consumer. */ public static Command fromSubscription(Supplier resource) { - return Command.just(() -> { + return Command.just(x -> { Disposable disposable = resource.get(); return Flowable.just(new TerminatorWrapper(disposable::dispose)); }); @@ -73,17 +71,16 @@ public static Command parallel(Command... commands) { } public static Command fromSwitch(Publisher commandStream) { - Flowable commands = Flowable.defer(() -> - Flowable.switchOnNext(Flowable.fromPublisher(commandStream).map(CommandWorkUnit::execute) - .subscribeOn(Schedulers.io()))); - return new Command(() -> commands.scan((a, b) -> { + return new Command(x -> Flowable.defer(() -> + Flowable.switchOnNext(Flowable.fromPublisher(commandStream).map(y -> y.execute(x)) + .subscribeOn(Schedulers.io()))).scan((a, b) -> { a.run(); return b; })); } public Command addFinalTerminator(Terminator terminator) { - return Command.just(() -> this.source.execute().flatMap(x -> Flowable.just(x, terminator))); + return Command.just(x -> this.source.execute(x).flatMap(y -> Flowable.just(y, terminator))); } /** @@ -91,7 +88,8 @@ public Command addFinalTerminator(Terminator terminator) { */ public Command startable(Publisher starter) { return new Command(new SerialCommand( - () -> Flowable.defer(() -> Flowable.never().takeUntil(starter)), this)); + Command.just(x -> Flowable.defer(() -> Flowable.never().takeUntil(starter))), + this)); } /** @@ -107,12 +105,10 @@ public Command when(Supplier startCondition) { * Copies this command into one which will end when terminator emits an item. */ public Command terminable(Publisher terminator) { - return new Command(() -> { - ConnectableFlowable flowterminator = Flowable.fromPublisher(terminator).publish(); - flowterminator.connect(); - Flowable sourceTerminator = this.execute(); + return new Command(x -> { + Flowable sourceTerminator = this.execute(x); Terminator multi = FlattenedTerminator.from(sourceTerminator); - return Flowable.defer(() -> Flowable.never().takeUntil(flowterminator) + return Flowable.defer(() -> Flowable.never().takeUntil(terminator) .mergeWith(Flowable.just(multi::run))); }); } @@ -122,8 +118,9 @@ public Command terminable(Publisher terminator) { * will only complete once endCondition returns true. */ public Command until(Supplier endCondition) { - Flowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x).onBackpressureDrop().publish(); - return new Command(this::execute).terminable(terminator); + ConnectableFlowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x).onBackpressureDrop().publish(); + terminator.connect(); + return this.terminable(terminator); } /** @@ -141,8 +138,8 @@ public Command delayStart(long delay, TimeUnit unit) { } public Command killAfter(long delay, TimeUnit unit) { - return Command.just(() -> { - Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(); + return Command.just(x -> { + Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(x); return terms.doOnComplete(() -> { FlattenedTerminator.from(terms).toSingle() .subscribe(Terminator::run); @@ -151,14 +148,11 @@ public Command killAfter(long delay, TimeUnit unit) { } @Override - public Flowable execute() { - Flowable terms = source.execute(); - return terms; - } - - public Flowable startExecution() { - Flowable terms = this.execute().subscribeOn(Schedulers.io()); - terms.toList().subscribe(x -> Observable.fromIterable(x).blockingSubscribe(Terminator::run)); + public Flowable execute(boolean selfTerminating) { + Flowable terms = source.execute(selfTerminating).subscribeOn(Schedulers.io()); + if (selfTerminating) { + terms.toList().subscribe(x -> Observable.fromIterable(x).blockingSubscribe(Terminator::run)); + } return terms; } } diff --git a/src/com/nutrons/framework/commands/CommandWorkUnit.java b/src/com/nutrons/framework/commands/CommandWorkUnit.java index 9f31088..8458be0 100644 --- a/src/com/nutrons/framework/commands/CommandWorkUnit.java +++ b/src/com/nutrons/framework/commands/CommandWorkUnit.java @@ -3,5 +3,5 @@ import io.reactivex.Flowable; public interface CommandWorkUnit { - Flowable execute(); + Flowable execute(boolean selfTerminating); } diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index 5e73ccb..df227ad 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -4,18 +4,17 @@ import io.reactivex.schedulers.Schedulers; public class ParallelCommand implements CommandWorkUnit { - private final Flowable commands; + private final Flowable commands; ParallelCommand(CommandWorkUnit... commands) { - this.commands = Flowable.fromArray(commands); - + this.commands = Flowable.fromArray(commands).map(Command::just); } @Override - public Flowable execute() { - Flowable terminators = this.commands.flatMap(x -> x.execute().subscribeOn(Schedulers.io())) + public Flowable execute(boolean selfTerminating) { + Flowable terminators = this.commands + .flatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); - return Flowable.just(FlattenedTerminator.from(terminators)) .mergeWith(terminators.ignoreElements().toFlowable()); } diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index a55266b..0914471 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -4,18 +4,17 @@ import io.reactivex.schedulers.Schedulers; public class SerialCommand implements CommandWorkUnit { - private final Flowable commands; + private final Flowable commands; SerialCommand(CommandWorkUnit... commands) { - this.commands = Flowable.fromArray(commands); - + this.commands = Flowable.fromArray(commands).map(Command::just); } @Override - public Flowable execute() { - Flowable terminators = this.commands.concatMap(x -> x.execute().subscribeOn(Schedulers.io())) + public Flowable execute(boolean selfTerminating) { + Flowable terminators = this.commands + .concatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); - return Flowable.just(FlattenedTerminator.from(terminators)) .mergeWith(terminators.ignoreElements().toFlowable()); } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index 18ff929..9fafab6 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -35,22 +35,23 @@ public void single() { arr[0] = 5; Command command = Command.fromAction(() -> arr[0] = 10); // Tests to see if this single command works. - waitForCommand(command.execute()); + waitForCommand(command.execute(true)); assertTrue(arr[0] == 10); } @Test public void testDelay() { long start = System.currentTimeMillis(); - waitForCommand(delay.execute()); + waitForCommand(delay.execute(true)); assertTrue(System.currentTimeMillis() - 1000 > start); + assertTrue(System.currentTimeMillis() - 2000 < start); } @Test public void inSeriesTimed() { long start = System.currentTimeMillis(); Command series = delay.then(delay); - waitForCommand(series.execute()); + waitForCommand(series.execute(true)); assertTrue(System.currentTimeMillis() - 2000 > start); assertTrue(System.currentTimeMillis() - 3000 < start); } @@ -59,17 +60,18 @@ public void inSeriesTimed() { public void inParallelTimed() { long start = System.currentTimeMillis(); Command para = Command.parallel(delay, delay); - waitForCommand(para.execute()); + waitForCommand(para.execute(true)); assertTrue(System.currentTimeMillis() - 1400 < start); } @Test public void testTerminable() throws InterruptedException { long start = System.currentTimeMillis(); - PublishProcessor pp = PublishProcessor.create(); + PublishProcessor pp = PublishProcessor.create(); Flowable d = serial(delay, delay, delay, delay) - .terminable(pp).execute(); + .terminable(pp).execute(true); Thread.sleep(3000); + pp.onNext(new Object()); pp.onComplete(); waitForCommand(d); assertTrue(System.currentTimeMillis() - 4000 < start); @@ -80,7 +82,7 @@ public void testUntil() throws InterruptedException { int[] record = new int[2]; assertTrue(record[0] == 0); long start = System.currentTimeMillis(); - Flowable d = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1).execute(); + Flowable d = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1).execute(true); Flowable.timer(1, TimeUnit.SECONDS).subscribeOn(Schedulers.io()).subscribe(x -> record[1] = 1); waitForCommand(d); assertTrue(System.currentTimeMillis() - 1000 > start); @@ -95,7 +97,7 @@ public void testStartable() { long start = System.currentTimeMillis(); waitForCommand(Command.fromAction(() -> { }) - .startable(Flowable.timer(1, TimeUnit.SECONDS)).execute()); + .startable(Flowable.timer(1, TimeUnit.SECONDS)).execute(true)); assertTrue(System.currentTimeMillis() - 1000 > start); } @@ -103,7 +105,7 @@ public void testStartable() { public void testWhen() throws InterruptedException { int[] record = new int[2]; assertTrue(record[0] == 0); - Flowable d = Command.fromAction(() -> record[0] = 1).when(() -> record[1] == 1).execute(); + Flowable d = Command.fromAction(() -> record[0] = 1).when(() -> record[1] == 1).execute(true); Thread.sleep(1000); assertTrue(record[0] == 0); long start = System.currentTimeMillis(); @@ -116,7 +118,7 @@ public void testWhen() throws InterruptedException { @Test public void parallelAndSerial() { long start = System.currentTimeMillis(); - waitForCommand(parallel(delay.then(delay), delay).execute()); + waitForCommand(parallel(delay.then(delay), delay).execute(true)); assertTrue(System.currentTimeMillis() - 2000 > start); assertTrue(System.currentTimeMillis() - 3000 < start); } @@ -125,11 +127,10 @@ public void parallelAndSerial() { public void killAfter() throws InterruptedException { int[] record = new int[1]; long start = System.currentTimeMillis(); - Command.just(() -> Flowable.just(() -> { - System.out.println("hi"); + Command.just(x -> Flowable.just(() -> { assertTrue(System.currentTimeMillis() - 2000 < start); record[0] = 1; - })).killAfter(1, TimeUnit.SECONDS).startExecution(); + })).killAfter(1, TimeUnit.SECONDS).execute(true); Thread.sleep(4000); assertTrue(record[0] == 1); } @@ -138,7 +139,7 @@ public void killAfter() throws InterruptedException { public void testSwitch() throws InterruptedException { int[] record = new int[1]; record[0] = 0; - Command inc = Command.just(() -> { + Command inc = Command.just(x -> { synchronized (record) { record[0] += 1; } @@ -149,7 +150,7 @@ public void testSwitch() throws InterruptedException { })); }); Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) - .execute().blockingSubscribe(Terminator::run); + .execute(true).blockingSubscribe(Terminator::run); Thread.sleep(2000); assertTrue(record[0] == 0); } diff --git a/test/com/nutrons/framework/test/TestCommandReuse.java b/test/com/nutrons/framework/test/TestCommandReuse.java index 56da822..3723539 100644 --- a/test/com/nutrons/framework/test/TestCommandReuse.java +++ b/test/com/nutrons/framework/test/TestCommandReuse.java @@ -19,13 +19,13 @@ public void testEndConditionReuse() { record[1] = 0; // if 1, the command will end. Command change = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1); waitForCommand(parallel(change, Command.fromAction(() -> record[1] = 1) - .delayStart(2, TimeUnit.SECONDS)).execute()); + .delayStart(2, TimeUnit.SECONDS)).execute(true)); assertTrue(record[0] == 1); // now we try it again. record[0] = 0; record[1] = 0; long start = System.currentTimeMillis(); - change.killAfter(1, TimeUnit.SECONDS).execute().blockingSubscribe(); + change.killAfter(1, TimeUnit.SECONDS).execute(true).blockingSubscribe(); // assert that command only quit because 1 second passed, proving that the command is reusable. assertTrue(start + 900 < System.currentTimeMillis()); // assert that command still functioned @@ -40,10 +40,10 @@ public void testIncrementReuse() throws InterruptedException { record[0] += 1; } }); - serial(inc, inc, inc).execute(); + serial(inc, inc, inc).execute(true); Thread.sleep(3000); assertTrue(record[0] == 3); - inc.execute(); + inc.execute(true); Thread.sleep(1000); assertTrue(record[0] == 4); } From 46f241064b992c3c73de6c7a5b0e2669b10a4a30 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Mon, 20 Feb 2017 11:41:54 -0500 Subject: [PATCH 14/45] more bug fixes and tests --- .../nutrons/framework/commands/Command.java | 29 +++++++-- .../framework/test/MultiCommandTest.java | 64 +++++++++++++++++++ .../nutrons/framework/test/TestCommand.java | 26 +------- 3 files changed, 89 insertions(+), 30 deletions(-) create mode 100644 test/com/nutrons/framework/test/MultiCommandTest.java diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index dccaeb6..755e9f9 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -102,11 +102,22 @@ public Command when(Supplier startCondition) { } /** - * Copies this command into one which will end when terminator emits an item. + * Copies this command into one which will end and terminate + * only when the terminator emits an item. */ public Command terminable(Publisher terminator) { - return new Command(x -> { - Flowable sourceTerminator = this.execute(x); + return this.endsWhen(terminator, false); + } + + /** + * Copies this command into one which will end when terminator emits an item. + * + * @param terminatesAtEnd if true, the command will terminate only when the end is reached, + * if false, the command may terminate before the end is reached. + */ + public Command endsWhen(Publisher terminator, boolean terminatesAtEnd) { + return Command.just(x -> { + Flowable sourceTerminator = this.execute(terminatesAtEnd); Terminator multi = FlattenedTerminator.from(sourceTerminator); return Flowable.defer(() -> Flowable.never().takeUntil(terminator) .mergeWith(Flowable.just(multi::run))); @@ -137,13 +148,17 @@ public Command delayStart(long delay, TimeUnit unit) { return this.startable(Flowable.timer(delay, unit)); } + /** + * Copies this command into one which will delay termination until a period of time has passed. + */ + public Command delayFinish(long delay, TimeUnit unit) { + return this.terminable(Flowable.timer(delay, unit)); + } + public Command killAfter(long delay, TimeUnit unit) { return Command.just(x -> { Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(x); - return terms.doOnComplete(() -> { - FlattenedTerminator.from(terms).toSingle() - .subscribe(Terminator::run); - }); + return terms; }); } diff --git a/test/com/nutrons/framework/test/MultiCommandTest.java b/test/com/nutrons/framework/test/MultiCommandTest.java new file mode 100644 index 0000000..510e164 --- /dev/null +++ b/test/com/nutrons/framework/test/MultiCommandTest.java @@ -0,0 +1,64 @@ +package com.nutrons.framework.test; + +import com.nutrons.framework.commands.Command; +import com.nutrons.framework.commands.TerminatorWrapper; +import io.reactivex.Flowable; +import org.junit.Before; +import org.junit.Test; + +import java.util.concurrent.TimeUnit; + +import static com.nutrons.framework.commands.Command.parallel; +import static junit.framework.TestCase.assertTrue; + +public class MultiCommandTest { + private Command delay; + + @Before + public void setupCommands() { + delay = Command.fromAction(() -> { + }).killAfter(1000, TimeUnit.MILLISECONDS); + } + + @Test + public void testSerialAndParallel() { + long start = System.currentTimeMillis(); + parallel(delay.then(delay), delay).execute(true).blockingSubscribe(); + assertTrue(System.currentTimeMillis() - start < 3000); + assertTrue(System.currentTimeMillis() - start > 2000); + } + + @Test + public void testOneThenAnother() throws InterruptedException { + int[] record = new int[1]; + Command oneThenZero = Command.just(x -> { + record[0] = 1; + return Flowable.just(() -> record[0] = 0); + }); + oneThenZero.delayFinish(2000, TimeUnit.MILLISECONDS).execute(true); + Thread.sleep(1000); + assertTrue(record[0] == 1); + Thread.sleep(2000); + assertTrue(record[0] == 0); + } + + @Test + public void testSwitch() throws InterruptedException { + int[] record = new int[1]; + record[0] = 0; + Command inc = Command.just(x -> { + synchronized (record) { + record[0] += 1; + } + return Flowable.just(new TerminatorWrapper(() -> { + synchronized (record) { + record[0] -= 1; + } + })); + }); + Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) + .execute(true).blockingSubscribe(); + Thread.sleep(2000); + assertTrue(record[0] == 0); + } +} diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index 9fafab6..d938c86 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -2,7 +2,6 @@ import com.nutrons.framework.commands.Command; import com.nutrons.framework.commands.Terminator; -import com.nutrons.framework.commands.TerminatorWrapper; import io.reactivex.Flowable; import io.reactivex.processors.PublishProcessor; import io.reactivex.schedulers.Schedulers; @@ -128,30 +127,11 @@ public void killAfter() throws InterruptedException { int[] record = new int[1]; long start = System.currentTimeMillis(); Command.just(x -> Flowable.just(() -> { - assertTrue(System.currentTimeMillis() - 2000 < start); + assertTrue(System.currentTimeMillis() - 3000 < start); + assertTrue(System.currentTimeMillis() - 1000 > start); record[0] = 1; - })).killAfter(1, TimeUnit.SECONDS).execute(true); + })).killAfter(2, TimeUnit.SECONDS).execute(true); Thread.sleep(4000); assertTrue(record[0] == 1); } - - @Test - public void testSwitch() throws InterruptedException { - int[] record = new int[1]; - record[0] = 0; - Command inc = Command.just(x -> { - synchronized (record) { - record[0] += 1; - } - return Flowable.just(new TerminatorWrapper(() -> { - synchronized (record) { - record[0] -= 1; - } - })); - }); - Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) - .execute(true).blockingSubscribe(Terminator::run); - Thread.sleep(2000); - assertTrue(record[0] == 0); - } } From 68bf785c3473e1f2f8d83c9b353b2cf7a0173514 Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Mon, 20 Feb 2017 12:16:36 -0500 Subject: [PATCH 15/45] fixed streammanager bug --- src/com/nutrons/framework/StreamManager.java | 8 ++++---- src/com/nutrons/framework/commands/Command.java | 5 ++++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index 54f72fd..8c91302 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -65,8 +65,8 @@ public final void startCompetition(Supplier autoSupplier, Supplier { System.out.println("Starting Autonomous: " + auto.getClass().toString()); return a; - }).subscribe(x -> auto.terminable(mode.filter(y -> y != AUTO) - .mergeWith(enabled.filter(z -> !z).map(z -> AUTO))).execute(true)); + }).subscribe(x -> auto.endsWhen(mode.filter(y -> y != AUTO) + .mergeWith(enabled.filter(z -> !z).map(z -> AUTO)), true).execute(true)); } Command tele = teleopSupplier.get(); if (tele != null) { @@ -76,8 +76,8 @@ public final void startCompetition(Supplier autoSupplier, Supplier { System.out.println("Starting Teleop: " + tele.getClass().toString()); return a; - }).subscribe(x -> tele.terminable(mode.filter(y -> y != TELE) - .mergeWith(enabled.filter(z -> !z).map(z -> TELE))).execute(true)); + }).subscribe(x -> tele.endsWhen(mode.filter(y -> y != TELE) + .mergeWith(enabled.filter(z -> !z).map(z -> TELE)), true).execute(true)); } this.enabled.ignoreElements().blockingAwait(); this.mode.ignoreElements().blockingAwait(); diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 755e9f9..0c96a9e 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -1,7 +1,9 @@ package com.nutrons.framework.commands; +import com.nutrons.framework.util.FlowOperators; import io.reactivex.Flowable; import io.reactivex.Observable; +import io.reactivex.Scheduler; import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.schedulers.Schedulers; @@ -10,6 +12,7 @@ import java.util.concurrent.TimeUnit; import java.util.function.Supplier; +import static com.nutrons.framework.util.FlowOperators.printId; import static com.nutrons.framework.util.FlowOperators.toFlow; public class Command implements CommandWorkUnit { @@ -80,7 +83,7 @@ public static Command fromSwitch(Publisher commandStr } public Command addFinalTerminator(Terminator terminator) { - return Command.just(x -> this.source.execute(x).flatMap(y -> Flowable.just(y, terminator))); + return Command.just(x -> this.source.execute(x).flatMap(y -> Flowable.just(y, terminator)).subscribeOn(Schedulers.io())); } /** From 87a6da081ecd556e6362e137985a6095907e84d0 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Mon, 20 Feb 2017 14:01:46 -0500 Subject: [PATCH 16/45] fixed climbtake bug --- .../framework/controllers/LoopSpeedController.java | 4 ++++ src/com/nutrons/framework/controllers/Talon.java | 10 ++++++++++ .../framework/controllers/VirtualSpeedController.java | 10 ++++++++++ 3 files changed, 24 insertions(+) diff --git a/src/com/nutrons/framework/controllers/LoopSpeedController.java b/src/com/nutrons/framework/controllers/LoopSpeedController.java index a8cba87..4983801 100644 --- a/src/com/nutrons/framework/controllers/LoopSpeedController.java +++ b/src/com/nutrons/framework/controllers/LoopSpeedController.java @@ -43,4 +43,8 @@ public void setReversedSensor(boolean flipped) { public abstract boolean fwdLimitSwitchClosed(); public abstract boolean revLimitSwitchClosed(); + + public abstract void noSticky(); + + public abstract void enableControl(); } diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index 26acf27..9f23488 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -147,4 +147,14 @@ public double getClosedLoopError() { public void reverseOutput(boolean flip){ this.talon.reverseOutput(flip); } + + public void noSticky(){ + this.talon.clearStickyFaults(); + } + + public void enableControl(){ + this.talon.enable(); + this.talon.enableControl(); + + } } diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index 363d973..bd8fdfe 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -28,7 +28,17 @@ public boolean revLimitSwitchClosed() { return false; } + @Override + public void noSticky() { + + } + public double speed() { return 0; } + + + public void enableControl(){ + + } } From f304ec37fc2db99041729437d4505655ea7bdd4d Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Mon, 20 Feb 2017 22:40:30 -0500 Subject: [PATCH 17/45] step 1 completed --- .idea/FRamework.iml | 9 + build.xml | 8 +- helper.xml | 6 +- ivy.xml | 1 + src/com/nutrons/framework/inputs/Serial.java | 2 +- .../nutrons/framework/util/FlowOperators.java | 2 +- .../nutrons/libKudos254/vision/AdbBridge.java | 80 ++++++ .../libKudos254/vision/CrashTracker.java | 66 +++++ .../vision/CrashTrackingRunnable.java | 19 ++ .../libKudos254/vision/TargetInfo.java | 28 +++ .../libKudos254/vision/VisionServer.java | 231 ++++++++++++++++++ .../libKudos254/vision/VisionServerTest.java | 29 +++ .../libKudos254/vision/VisionUpdate.java | 102 ++++++++ .../vision/VisionUpdateReceiver.java | 11 + .../vision/messages/HeartbeatMessage.java | 27 ++ .../vision/messages/OffWireMessage.java | 41 ++++ .../vision/messages/SetCameraModeMessage.java | 35 +++ .../vision/messages/VisionMessage.java | 21 ++ 18 files changed, 709 insertions(+), 9 deletions(-) create mode 100644 src/com/nutrons/libKudos254/vision/AdbBridge.java create mode 100644 src/com/nutrons/libKudos254/vision/CrashTracker.java create mode 100644 src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java create mode 100644 src/com/nutrons/libKudos254/vision/TargetInfo.java create mode 100644 src/com/nutrons/libKudos254/vision/VisionServer.java create mode 100644 src/com/nutrons/libKudos254/vision/VisionServerTest.java create mode 100644 src/com/nutrons/libKudos254/vision/VisionUpdate.java create mode 100644 src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java create mode 100644 src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java create mode 100644 src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java create mode 100644 src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java create mode 100644 src/com/nutrons/libKudos254/vision/messages/VisionMessage.java diff --git a/.idea/FRamework.iml b/.idea/FRamework.iml index 8696830..0a84278 100644 --- a/.idea/FRamework.iml +++ b/.idea/FRamework.iml @@ -12,5 +12,14 @@ + + + + + + + + + \ No newline at end of file diff --git a/build.xml b/build.xml index 8306937..9c1132d 100644 --- a/build.xml +++ b/build.xml @@ -29,9 +29,9 @@ + it into ant's libKudos254 dir (note that the latter copy will always take precedence). + We will not fail as long as local libKudos254 dir exists (it may be empty) and + ivy is in at least one of ant's libKudos254 dir or the local libKudos254 dir. --> @@ -53,7 +53,7 @@ - + diff --git a/helper.xml b/helper.xml index 00c756d..b2f063c 100644 --- a/helper.xml +++ b/helper.xml @@ -21,9 +21,9 @@ + it into ant's libKudos254 dir (note that the latter copy will always take precedence). + We will not fail as long as local libKudos254 dir exists (it may be empty) and + ivy is in at least one of ant's libKudos254 dir or the local libKudos254 dir. --> diff --git a/ivy.xml b/ivy.xml index de7ca05..232299b 100644 --- a/ivy.xml +++ b/ivy.xml @@ -13,5 +13,6 @@ + diff --git a/src/com/nutrons/framework/inputs/Serial.java b/src/com/nutrons/framework/inputs/Serial.java index 3639ec3..ca6e721 100644 --- a/src/com/nutrons/framework/inputs/Serial.java +++ b/src/com/nutrons/framework/inputs/Serial.java @@ -82,7 +82,7 @@ public Serial(int baudrate, } return serial.read(packetLength); }; - this.dataStream = toFlowFast(new IntervalCache(50, supplier)) + this.dataStream = toFlowFast(new IntervalCache(100, supplier)) .filter(x -> x.length == packetLength); } catch (RuntimeException e) { System.out.println(e.getMessage()); diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 99ba114..99f368f 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -43,7 +43,7 @@ public static Flowable toFlow(Supplier supplier) { } public static Flowable toFlowFast(Supplier supplier) { - return toFlow(supplier, 50, TimeUnit.MILLISECONDS); + return toFlow(supplier, 100, TimeUnit.MILLISECONDS); } /** diff --git a/src/com/nutrons/libKudos254/vision/AdbBridge.java b/src/com/nutrons/libKudos254/vision/AdbBridge.java new file mode 100644 index 0000000..a5e07ef --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/AdbBridge.java @@ -0,0 +1,80 @@ +package com.nutrons.libKudos254.vision; + +import java.io.IOException; +import java.nio.file.Path; +import java.nio.file.Paths; + +/** + * AdbBridge interfaces to an Android Debug Bridge (adb) binary, which is needed + * to communicate to Android devices over USB. + * + * adb binary provided by https://github.com/Spectrum3847/RIOdroid + */ +public class AdbBridge { + Path bin_location_; + public final static Path DEFAULT_LOCATION = Paths.get("/usr/bin/adb"); + + public AdbBridge() { + Path adb_location; + String env_val = System.getenv("FRC_ADB_LOCATION"); + if (env_val == null || "".equals(env_val)) { + adb_location = DEFAULT_LOCATION; + } else { + adb_location = Paths.get(env_val); + } + bin_location_ = adb_location; + } + + public AdbBridge(Path location) { + bin_location_ = location; + } + + private boolean runCommand(String args) { + Runtime r = Runtime.getRuntime(); + String cmd = bin_location_.toString() + " " + args; + + try { + Process p = r.exec(cmd); + p.waitFor(); + } catch (IOException e) { + System.err.println("AdbBridge: Could not run command " + cmd); + e.printStackTrace(); + return false; + } catch (InterruptedException e) { + System.err.println("AdbBridge: Could not run command " + cmd); + e.printStackTrace(); + return false; + } + return true; + } + + public void start() { + System.out.println("Starting adb"); + runCommand("start"); + } + + public void stop() { + System.out.println("Stopping adb"); + runCommand("kill-server"); + } + + public void restartAdb() { + System.out.println("Restarting adb"); + stop(); + start(); + } + + public void portForward(int local_port, int remote_port) { + runCommand("forward tcp:" + local_port + " tcp:" + remote_port); + } + + public void reversePortForward(int remote_port, int local_port) { + runCommand("reverse tcp:" + remote_port + " tcp:" + local_port); + } + + public void restartApp() { + System.out.println("Restarting app"); + runCommand("shell am force-stop com.team254.cheezdroid \\; " + + "am start com.team254.cheezdroid/com.team254.cheezdroid.VisionTrackerActivity"); + } +} diff --git a/src/com/nutrons/libKudos254/vision/CrashTracker.java b/src/com/nutrons/libKudos254/vision/CrashTracker.java new file mode 100644 index 0000000..6ddf767 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/CrashTracker.java @@ -0,0 +1,66 @@ +package com.nutrons.libKudos254.vision; + +import java.io.*; +import java.util.Date; +import java.util.UUID; + +/** + * Tracks start-up and caught crash events, logging them to a file which dosn't + * roll over + */ +public class CrashTracker { + + private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID(); + + public static void logRobotStartup() { + logMarker("robot startup"); + } + + public static void logRobotConstruction() { + logMarker("robot startup"); + } + + public static void logRobotInit() { + logMarker("robot init"); + } + + public static void logTeleopInit() { + logMarker("teleop init"); + } + + public static void logAutoInit() { + logMarker("auto init"); + } + + public static void logDisabledInit() { + logMarker("disabled init"); + } + + public static void logThrowableCrash(Throwable throwable) { + logMarker("Exception", throwable); + } + + private static void logMarker(String mark) { + logMarker(mark, null); + } + + private static void logMarker(String mark, Throwable nullableException) { + + try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) { + writer.print(RUN_INSTANCE_UUID.toString()); + writer.print(", "); + writer.print(mark); + writer.print(", "); + writer.print(new Date().toString()); + + if (nullableException != null) { + writer.print(", "); + nullableException.printStackTrace(writer); + } + + writer.println(); + } catch (IOException e) { + e.printStackTrace(); + } + } +} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java b/src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java new file mode 100644 index 0000000..ce4bdf4 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java @@ -0,0 +1,19 @@ +package com.nutrons.libKudos254.vision; + +/** + * Runnable class with reports all uncaught throws to CrashTracker + */ +public abstract class CrashTrackingRunnable implements Runnable { + + @Override + public final void run() { + try { + runCrashTracked(); + } catch (Throwable t) { + CrashTracker.logThrowableCrash(t); + throw t; + } + } + + public abstract void runCrashTracked(); +} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/TargetInfo.java b/src/com/nutrons/libKudos254/vision/TargetInfo.java new file mode 100644 index 0000000..7a2903d --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/TargetInfo.java @@ -0,0 +1,28 @@ +package com.nutrons.libKudos254.vision; + +/** + * A container class for Targets detected by the vision system, containing the + * location in three-dimensional space. + */ +public class TargetInfo { + protected double x = 1.0; + protected double y; + protected double z; + + public TargetInfo(double y, double z) { + this.y = y; + this.z = z; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getZ() { + return z; + } +} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/VisionServer.java b/src/com/nutrons/libKudos254/vision/VisionServer.java new file mode 100644 index 0000000..98eb1d4 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/VisionServer.java @@ -0,0 +1,231 @@ +package com.nutrons.libKudos254.vision; + +import com.nutrons.libKudos254.vision.messages.HeartbeatMessage; +import com.nutrons.libKudos254.vision.messages.OffWireMessage; +import com.nutrons.libKudos254.vision.messages.VisionMessage; +import edu.wpi.first.wpilibj.Timer; + +import java.io.IOException; +import java.io.InputStream; +import java.io.OutputStream; +import java.net.*; +import java.util.ArrayList; +import java.util.Collections; + +/** + * This controls all vision actions, including vision updates, capture, and + * interfacing with the Android phone with Android Debug Bridge. It also stores + * all VisionUpdates (from the Android phone) and contains methods to add + * to/prune the VisionUpdate list. Much like the subsystems, outside methods get + * the VisionServer instance (there is only one VisionServer) instead of + * creating new VisionServer instances. + * + * @see VisionUpdate.java + */ + +public class VisionServer extends CrashTrackingRunnable { + + public static int kAndroidAppTcpPort = 8254; //TODO: MIGHT NEED TO CHANGE THIS + + private static VisionServer s_instance = null; + private ServerSocket m_server_socket; + private boolean m_running = true; + private int m_port; + private ArrayList receivers = new ArrayList<>(); + AdbBridge adb = new AdbBridge(); + double lastMessageReceivedTime = 0; + private boolean m_use_java_time = false; + + private ArrayList serverThreads = new ArrayList<>(); + private volatile boolean mWantsAppRestart = false; + + public static VisionServer getInstance() { + if (s_instance == null) { + s_instance = new VisionServer(kAndroidAppTcpPort); + } + return s_instance; + } + + private boolean mIsConnect = false; + + public boolean isConnected() { + return mIsConnect; + } + + public void requestAppRestart() { + mWantsAppRestart = true; + } + + protected class ServerThread extends CrashTrackingRunnable { + private Socket m_socket; + + public ServerThread(Socket socket) { + m_socket = socket; + } + + public void send(VisionMessage message) { + String toSend = message.toJson() + "\n"; + if (m_socket != null && m_socket.isConnected()) { + try { + OutputStream os = m_socket.getOutputStream(); + os.write(toSend.getBytes()); + } catch (IOException e) { + System.err.println("VisionServer: Could not send data to socket"); + } + } + } + + public void handleMessage(VisionMessage message, double timestamp) { + if ("targets".equals(message.getType())) { + VisionUpdate update = VisionUpdate.generateFromJsonString(timestamp, message.getMessage()); + receivers.removeAll(Collections.singleton(null)); + if (update.isValid()) { + for (VisionUpdateReceiver receiver : receivers) { + receiver.gotUpdate(update); + } + } + } + if ("heartbeat".equals(message.getType())) { + send(HeartbeatMessage.getInstance()); + } + } + + public boolean isAlive() { + return m_socket != null && m_socket.isConnected() && !m_socket.isClosed(); + } + + @Override + public void runCrashTracked() { + if (m_socket == null) { + return; + } + try { + InputStream is = m_socket.getInputStream(); + byte[] buffer = new byte[2048]; + int read; + while (m_socket.isConnected() && (read = is.read(buffer)) != -1) { + double timestamp = getTimestamp(); + lastMessageReceivedTime = timestamp; + String messageRaw = new String(buffer, 0, read); + String[] messages = messageRaw.split("\n"); + for (String message : messages) { + OffWireMessage parsedMessage = new OffWireMessage(message); + if (parsedMessage.isValid()) { + handleMessage(parsedMessage, timestamp); + } + } + } + System.out.println("Socket disconnected"); + } catch (IOException e) { + System.err.println("Could not talk to socket"); + } + if (m_socket != null) { + try { + m_socket.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + } + } + + /** + * Instantializes the VisionServer and connects to ADB via the specified + * port. + * + * @param Port + */ + private VisionServer(int port) { + try { + adb = new AdbBridge(); + m_port = port; + m_server_socket = new ServerSocket(port); + adb.start(); + adb.reversePortForward(port, port); + try { + String useJavaTime = System.getenv("USE_JAVA_TIME"); + m_use_java_time = "true".equals(useJavaTime); + } catch (NullPointerException e) { + m_use_java_time = false; + } + } catch (IOException e) { + e.printStackTrace(); + } + new Thread(this).start(); + new Thread(new AppMaintainanceThread()).start(); + } + + public void restartAdb() { + adb.restartAdb(); + adb.reversePortForward(m_port, m_port); + } + + /** + * If a VisionUpdate object (i.e. a target) is not in the list, add it. + * + * @see VisionUpdate + */ + public void addVisionUpdateReceiver(VisionUpdateReceiver receiver) { + if (!receivers.contains(receiver)) { + receivers.add(receiver); + } + } + + public void removeVisionUpdateReceiver(VisionUpdateReceiver receiver) { + if (receivers.contains(receiver)) { + receivers.remove(receiver); + } + } + + @Override + public void runCrashTracked() { + while (m_running) { + try { + Socket p = m_server_socket.accept(); + ServerThread s = new ServerThread(p); + new Thread(s).start(); + serverThreads.add(s); + } catch (IOException e) { + System.err.println("Issue accepting socket connection!"); + } finally { + try { + Thread.sleep(100); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } + } + + private class AppMaintainanceThread extends CrashTrackingRunnable { + @Override + public void runCrashTracked() { + while (true) { + if (getTimestamp() - lastMessageReceivedTime > .1) { + // camera disconnected + adb.reversePortForward(m_port, m_port); + mIsConnect = false; + } else { + mIsConnect = true; + } + if (mWantsAppRestart) { + adb.restartApp(); + mWantsAppRestart = false; + } + try { + Thread.sleep(200); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } + } + + private double getTimestamp() { + if (m_use_java_time) { + return System.currentTimeMillis(); + } else { + return Timer.getFPGATimestamp(); + } + } +} diff --git a/src/com/nutrons/libKudos254/vision/VisionServerTest.java b/src/com/nutrons/libKudos254/vision/VisionServerTest.java new file mode 100644 index 0000000..7260117 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/VisionServerTest.java @@ -0,0 +1,29 @@ +package com.nutrons.libKudos254.vision; + +/** + * Tests the vision system by getting targets + */ +public class VisionServerTest { + public static class TestReceiver implements VisionUpdateReceiver { + @Override + public void gotUpdate(VisionUpdate update) { + System.out.println("num targets: " + update.getTargets().size()); + for (int i = 0; i < update.getTargets().size(); i++) { + TargetInfo target = update.getTargets().get(i); + System.out.println("Target: " + target.getY() + ", " + target.getZ()); + } + } + } + + public static void main(String[] args) { + VisionServer visionServer = VisionServer.getInstance(); + visionServer.addVisionUpdateReceiver(new TestReceiver()); + while (true) { + try { + Thread.sleep(100); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } +} diff --git a/src/com/nutrons/libKudos254/vision/VisionUpdate.java b/src/com/nutrons/libKudos254/vision/VisionUpdate.java new file mode 100644 index 0000000..a1aa122 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/VisionUpdate.java @@ -0,0 +1,102 @@ +package com.nutrons.libKudos254.vision; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; +import org.json.simple.parser.ParseException; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +/** + * VisionUpdate contains the various attributes outputted by the vision system, + * namely a list of targets and the timestamp at which it was captured. + */ +public class VisionUpdate { + protected boolean valid = false; + protected long capturedAgoMs; + protected List targets; + protected double capturedAtTimestamp = 0; + + private static long getOptLong(Object n, long defaultValue) { + if (n == null) { + return defaultValue; + } + return (long) n; + } + + private static JSONParser parser = new JSONParser(); + + private static Optional parseDouble(JSONObject j, String key) throws ClassCastException { + Object d = j.get(key); + if (d == null) { + return Optional.empty(); + } else { + return Optional.of((double) d); + } + } + + /** + * Generates a VisionUpdate object given a JSON blob and a timestamp. + * + * @param Capture + * timestamp + * @param JSON + * blob with update string, example: { "capturedAgoMs" : 100, + * "targets": [{"y": 5.4, "z": 5.5}] } + * @return VisionUpdate object + */ + // + public static VisionUpdate generateFromJsonString(double current_time, String updateString) { + VisionUpdate update = new VisionUpdate(); + try { + JSONObject j = (JSONObject) parser.parse(updateString); + long capturedAgoMs = getOptLong(j.get("capturedAgoMs"), 0); + if (capturedAgoMs == 0) { + update.valid = false; + return update; + } + update.capturedAgoMs = capturedAgoMs; + update.capturedAtTimestamp = current_time - capturedAgoMs / 1000.0; + JSONArray targets = (JSONArray) j.get("targets"); + ArrayList targetInfos = new ArrayList<>(targets.size()); + for (Object targetObj : targets) { + JSONObject target = (JSONObject) targetObj; + Optional y = parseDouble(target, "y"); + Optional z = parseDouble(target, "z"); + if (!(y.isPresent() && z.isPresent())) { + update.valid = false; + return update; + } + targetInfos.add(new TargetInfo(y.get(), z.get())); + } + update.targets = targetInfos; + update.valid = true; + } catch (ParseException e) { + System.err.println("Parse error: " + e); + System.err.println(updateString); + } catch (ClassCastException e) { + System.err.println("Data type error: " + e); + System.err.println(updateString); + } + return update; + } + + public List getTargets() { + return targets; + } + + public boolean isValid() { + return valid; + } + + public long getCapturedAgoMs() { + return capturedAgoMs; + } + + public double getCapturedAtTimestamp() { + return capturedAtTimestamp; + } + +} diff --git a/src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java b/src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java new file mode 100644 index 0000000..87a0c15 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java @@ -0,0 +1,11 @@ +package com.nutrons.libKudos254.vision; + +/** + * A basic interface for classes that get VisionUpdates. Classes that implement + * this interface specify what to do when VisionUpdates are received. + * + * @see VisionUpdate.java + */ +public interface VisionUpdateReceiver { + void gotUpdate(VisionUpdate update); +} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java b/src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java new file mode 100644 index 0000000..aee19ec --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java @@ -0,0 +1,27 @@ +package com.nutrons.libKudos254.vision.messages; + +/** + * A message that acts as a "heartbeat"- ensures that the vision system is + * working. The message simply contains the instance of the VisionServer object. + */ +public class HeartbeatMessage extends VisionMessage { + + static HeartbeatMessage sInst = null; + + public static HeartbeatMessage getInstance() { + if (sInst == null) { + sInst = new HeartbeatMessage(); + } + return sInst; + } + + @Override + public String getType() { + return "heartbeat"; + } + + @Override + public String getMessage() { + return "{}"; + } +} diff --git a/src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java b/src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java new file mode 100644 index 0000000..5bb42d4 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java @@ -0,0 +1,41 @@ +package com.nutrons.libKudos254.vision.messages; + +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; +import org.json.simple.parser.ParseException; + +/** + * Used to convert Strings into OffWireMessage objects, which can be interpreted + * as generic VisionMessages. + */ +public class OffWireMessage extends VisionMessage { + + private boolean mValid = false; + private String mType = "unknown"; + private String mMessage = "{}"; + + public OffWireMessage(String message) { + JSONParser parser = new JSONParser(); + try { + JSONObject j = (JSONObject) parser.parse(message); + mType = (String) j.get("type"); + mMessage = (String) j.get("message"); + mValid = true; + } catch (ParseException e) { + } + } + + public boolean isValid() { + return mValid; + } + + @Override + public String getType() { + return mType; + } + + @Override + public String getMessage() { + return mMessage; + } +} diff --git a/src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java b/src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java new file mode 100644 index 0000000..9ddefe7 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java @@ -0,0 +1,35 @@ +package com.nutrons.libKudos254.vision.messages; + +/** + * A Message that contains and can set the state of the camera and intake + * systems. + */ +public class SetCameraModeMessage extends VisionMessage { + + private static final String K_VISION_MODE = "vision"; + private static final String K_INTAKE_MODE = "intake"; + + private String mMessage = K_VISION_MODE; + + private SetCameraModeMessage(String message) { + mMessage = message; + } + + public static SetCameraModeMessage getVisionModeMessage() { + return new SetCameraModeMessage(K_VISION_MODE); + } + + public static SetCameraModeMessage getIntakeModeMessage() { + return new SetCameraModeMessage(K_INTAKE_MODE); + } + + @Override + public String getType() { + return "camera_mode"; + } + + @Override + public String getMessage() { + return mMessage; + } +} diff --git a/src/com/nutrons/libKudos254/vision/messages/VisionMessage.java b/src/com/nutrons/libKudos254/vision/messages/VisionMessage.java new file mode 100644 index 0000000..141b460 --- /dev/null +++ b/src/com/nutrons/libKudos254/vision/messages/VisionMessage.java @@ -0,0 +1,21 @@ +package com.nutrons.libKudos254.vision.messages; + +import org.json.simple.JSONObject; + +/** + * An abstract class used for messages about the vision subsystem. + */ +public abstract class VisionMessage { + + public abstract String getType(); + + public abstract String getMessage(); + + public String toJson() { + JSONObject j = new JSONObject(); + j.put("type", getType()); + j.put("message", getMessage()); + return j.toString(); + } + +} From ec2f76c0550bba79c3c2e851e5563ebbf84c9d42 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Tue, 21 Feb 2017 11:37:03 -0500 Subject: [PATCH 18/45] update variable stays null --- src/com/nutrons/libKudos254/vision/VisionServer.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/com/nutrons/libKudos254/vision/VisionServer.java b/src/com/nutrons/libKudos254/vision/VisionServer.java index 98eb1d4..331c69e 100644 --- a/src/com/nutrons/libKudos254/vision/VisionServer.java +++ b/src/com/nutrons/libKudos254/vision/VisionServer.java @@ -20,7 +20,7 @@ * the VisionServer instance (there is only one VisionServer) instead of * creating new VisionServer instances. * - * @see VisionUpdate.java + * @see */ public class VisionServer extends CrashTrackingRunnable { @@ -40,6 +40,7 @@ public class VisionServer extends CrashTrackingRunnable { private volatile boolean mWantsAppRestart = false; public static VisionServer getInstance() { + System.out.println("VisionServer instance created"); if (s_instance == null) { s_instance = new VisionServer(kAndroidAppTcpPort); } @@ -70,6 +71,7 @@ public void send(VisionMessage message) { OutputStream os = m_socket.getOutputStream(); os.write(toSend.getBytes()); } catch (IOException e) { + System.out.println("Vision server IOException"); System.err.println("VisionServer: Could not send data to socket"); } } @@ -133,7 +135,7 @@ public void runCrashTracked() { * Instantializes the VisionServer and connects to ADB via the specified * port. * - * @param Port + * @param */ private VisionServer(int port) { try { @@ -166,6 +168,7 @@ public void restartAdb() { * @see VisionUpdate */ public void addVisionUpdateReceiver(VisionUpdateReceiver receiver) { + System.out.println("added vision update receiver"); if (!receivers.contains(receiver)) { receivers.add(receiver); } From 5e5a31935a08ed3a35c2c18a9de0478e8d5873c7 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Tue, 21 Feb 2017 15:36:45 -0500 Subject: [PATCH 19/45] removed print statements and accidentally refactoring of build files --- build.xml | 8 ++--- helper.xml | 6 ++-- .../nutrons/framework/controllers/Talon.java | 3 -- .../nutrons/framework/test/TestCommand.java | 31 ------------------- 4 files changed, 7 insertions(+), 41 deletions(-) diff --git a/build.xml b/build.xml index 9c1132d..8306937 100644 --- a/build.xml +++ b/build.xml @@ -29,9 +29,9 @@ + it into ant's lib dir (note that the latter copy will always take precedence). + We will not fail as long as local lib dir exists (it may be empty) and + ivy is in at least one of ant's lib dir or the local lib dir. --> @@ -53,7 +53,7 @@ - + diff --git a/helper.xml b/helper.xml index b2f063c..00c756d 100644 --- a/helper.xml +++ b/helper.xml @@ -21,9 +21,9 @@ + it into ant's lib dir (note that the latter copy will always take precedence). + We will not fail as long as local lib dir exists (it may be empty) and + ivy is in at least one of ant's lib dir or the local lib dir. --> diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index fbcc331..5babd71 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -65,7 +65,6 @@ void changeControlMode(ControlMode mode) { } void changeSetpoint(double setpoint) { - System.out.println("Changing setpoint to " + setpoint); this.talon.setSetpoint(setpoint); } @@ -98,7 +97,6 @@ public void accept(ControllerEvent event) { @Override public void setOutputFlipped(boolean flipped) { - System.out.println("output: " + flipped); talon.setInverted(flipped); } @@ -112,7 +110,6 @@ public void setFeedbackDevice(CANTalon.FeedbackDevice device) { } void reverseSensor(boolean flipped) { - System.out.println("sensor: " + flipped); talon.reverseSensor(flipped); } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index b2d7d15..79014c9 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -132,36 +132,6 @@ public void parallelAndSerial() { public void killAfter() throws InterruptedException { int[] record = new int[1]; long start = System.currentTimeMillis(); -<<<<<<< HEAD - Command.just(() -> Flowable.just(() -> { - System.out.println("hi"); - assertTrue(System.currentTimeMillis() - 2000 < start); - record[0] = 1; - })).delayTermination(1000, TimeUnit.SECONDS).killAfter(1, TimeUnit.SECONDS).startExecution(); - Thread.sleep(4000); - assertTrue(record[0] == 1); - } - - @Test - public void testSwitch() throws InterruptedException { - int[] record = new int[1]; - record[0] = 0; - Command inc = Command.just(() -> { - synchronized (record) { - record[0] += 1; - } - return Flowable.just(new TerminatorWrapper(() -> { - synchronized (record) { - record[0] -= 1; - } - })); - }); - Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) - .execute().blockingSubscribe(Terminator::run); - Thread.sleep(2000); - assertTrue(record[0] == 0); - } -======= Command.just(x -> Flowable.just(() -> { assertTrue(System.currentTimeMillis() - 3000 < start); assertTrue(System.currentTimeMillis() - 1000 > start); @@ -170,5 +140,4 @@ public void testSwitch() throws InterruptedException { Thread.sleep(4000); assertTrue(record[0] == 1); } ->>>>>>> master } From 9b8bd9c4dc5e3d63371d4a0295330699b6d92bab Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Tue, 21 Feb 2017 16:07:35 -0500 Subject: [PATCH 20/45] added rotation2d for distance --- src/com/nutrons/libKudos254/Rotation2d.java | 113 ++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 src/com/nutrons/libKudos254/Rotation2d.java diff --git a/src/com/nutrons/libKudos254/Rotation2d.java b/src/com/nutrons/libKudos254/Rotation2d.java new file mode 100644 index 0000000..f346dba --- /dev/null +++ b/src/com/nutrons/libKudos254/Rotation2d.java @@ -0,0 +1,113 @@ +package com.nutrons.libKudos254; + +import java.text.DecimalFormat; + +/** + * A rotation in a 2d coordinate frame represented a point on the unit circle + * (cosine and sine). + * + * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) + */ +public class Rotation2d { + protected static final double kEpsilon = 1E-9; + + protected double cos_angle_; + protected double sin_angle_; + + public Rotation2d() { + this(1, 0, false); + } + + public Rotation2d(double x, double y, boolean normalize) { + cos_angle_ = x; + sin_angle_ = y; + if (normalize) { + normalize(); + } + } + + public Rotation2d(Rotation2d other) { + cos_angle_ = other.cos_angle_; + sin_angle_ = other.sin_angle_; + } + + public static Rotation2d fromRadians(double angle_radians) { + return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false); + } + + public static Rotation2d fromDegrees(double angle_degrees) { + return fromRadians(Math.toRadians(angle_degrees)); + } + + /** + * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this + * object we might accumulate rounding errors. Normalizing forces us to + * re-scale the sin and cos to reset rounding errors. + */ + public void normalize() { + double magnitude = Math.hypot(cos_angle_, sin_angle_); + if (magnitude > kEpsilon) { + sin_angle_ /= magnitude; + cos_angle_ /= magnitude; + } else { + sin_angle_ = 0; + cos_angle_ = 1; + } + } + + public double cos() { + return cos_angle_; + } + + public double sin() { + return sin_angle_; + } + + public double tan() { + if (cos_angle_ < kEpsilon) { + if (sin_angle_ >= 0.0) { + return Double.POSITIVE_INFINITY; + } else { + return Double.NEGATIVE_INFINITY; + } + } + return sin_angle_ / cos_angle_; + } + + public double getRadians() { + return Math.atan2(sin_angle_, cos_angle_); + } + + public double getDegrees() { + return Math.toDegrees(getRadians()); + } + + /** + * We can rotate this Rotation2d by adding together the effects of it and + * another rotation. + * + * @param other + * The other rotation. See: + * https://en.wikipedia.org/wiki/Rotation_matrix + * @return This rotation rotated by other. + */ + public Rotation2d rotateBy(Rotation2d other) { + return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_, + cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true); + } + + /** + * The inverse of a Rotation2d "undoes" the effect of this rotation. + * + * @return The opposite of this rotation. + */ + public Rotation2d inverse() { + return new Rotation2d(cos_angle_, -sin_angle_, false); + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(getDegrees()) + " deg)"; + } +} From f184476395de633079b22c8c808dbff24418979e Mon Sep 17 00:00:00 2001 From: Josh Young Date: Tue, 21 Feb 2017 19:43:00 -0500 Subject: [PATCH 21/45] Fixed Changes --- .../nutrons/framework/controllers/Events.java | 8 ++--- .../framework/controllers/RevServo.java | 19 +++++++++-- .../controllers/ServoController.java | 4 +-- .../{ServoEvent.java => ServoInstr.java} | 2 +- ...{SetAngleEvent.java => SetAngleInstr.java} | 6 ++-- .../framework/controllers/SetEvent.java | 33 ------------------- 6 files changed, 27 insertions(+), 45 deletions(-) rename src/com/nutrons/framework/controllers/{ServoEvent.java => ServoInstr.java} (86%) rename src/com/nutrons/framework/controllers/{SetAngleEvent.java => SetAngleInstr.java} (79%) delete mode 100644 src/com/nutrons/framework/controllers/SetEvent.java diff --git a/src/com/nutrons/framework/controllers/Events.java b/src/com/nutrons/framework/controllers/Events.java index 41ce8e4..9a8360e 100644 --- a/src/com/nutrons/framework/controllers/Events.java +++ b/src/com/nutrons/framework/controllers/Events.java @@ -95,11 +95,11 @@ public static ControllerEvent resetPosition(double position) { return new ResetPositionEvent(position); } - public static ServoEvent setAngle(double angle) { - return new SetAngleEvent(angle); + public static ServoInstr setAngle(double angle) { + return new SetAngleInstr(angle); } - public static ServoEvent set(double value) { - return new SetEvent(value); + public static ServoInstr set(double value) { + return new SetInstr(value); } } diff --git a/src/com/nutrons/framework/controllers/RevServo.java b/src/com/nutrons/framework/controllers/RevServo.java index bcc15e0..430f514 100644 --- a/src/com/nutrons/framework/controllers/RevServo.java +++ b/src/com/nutrons/framework/controllers/RevServo.java @@ -2,23 +2,38 @@ import edu.wpi.first.wpilibj.Servo; +/** + * The RevServo controlle used for the programming and running of Rev Smart Servos. + */ public class RevServo extends ServoController { private final Servo servo; + /** + * Makes a RevServo object at the given port. + * @param port Port of RevServo + */ public RevServo(int port) { servo = new Servo(port); } @Override - public void accept(ServoEvent servoEvent) { - servoEvent.actOn(this); + public void accept(ServoInstr servoInstr) { + servoInstr.actOn(this); } + /** + * Sets the Servo to a given angle ranging from -90 to 90 left and right reflectively. + * @param angle given angle to turn to. + */ public void setAngle(double angle) { this.servo.setAngle(angle); } + /** + * Set the motor to a position given a value 0.0 is full left 1.0 is full right. + * @param value The value to turn the servo to. + */ public void set(double value) { this.servo.set(value); } diff --git a/src/com/nutrons/framework/controllers/ServoController.java b/src/com/nutrons/framework/controllers/ServoController.java index 2d32876..a497de0 100644 --- a/src/com/nutrons/framework/controllers/ServoController.java +++ b/src/com/nutrons/framework/controllers/ServoController.java @@ -2,10 +2,10 @@ import io.reactivex.functions.Consumer; -public abstract class ServoController implements Consumer { +public abstract class ServoController implements Consumer { @Override - public abstract void accept(ServoEvent servoEvent); + public abstract void accept(ServoInstr servoInstr); public void setAngle(double angle) { this.accept(Events.setAngle(angle)); } diff --git a/src/com/nutrons/framework/controllers/ServoEvent.java b/src/com/nutrons/framework/controllers/ServoInstr.java similarity index 86% rename from src/com/nutrons/framework/controllers/ServoEvent.java rename to src/com/nutrons/framework/controllers/ServoInstr.java index 77617f5..30eab7a 100644 --- a/src/com/nutrons/framework/controllers/ServoEvent.java +++ b/src/com/nutrons/framework/controllers/ServoInstr.java @@ -1,6 +1,6 @@ package com.nutrons.framework.controllers; -public interface ServoEvent { +public interface ServoInstr { default void actOn(RevServo servo) { throw new EventUnimplementedException( diff --git a/src/com/nutrons/framework/controllers/SetAngleEvent.java b/src/com/nutrons/framework/controllers/SetAngleInstr.java similarity index 79% rename from src/com/nutrons/framework/controllers/SetAngleEvent.java rename to src/com/nutrons/framework/controllers/SetAngleInstr.java index 3e01f24..0d99807 100644 --- a/src/com/nutrons/framework/controllers/SetAngleEvent.java +++ b/src/com/nutrons/framework/controllers/SetAngleInstr.java @@ -1,6 +1,6 @@ package com.nutrons.framework.controllers; -public class SetAngleEvent implements ServoEvent { +public class SetAngleInstr implements ServoInstr { private final double angle; @@ -9,7 +9,7 @@ public class SetAngleEvent implements ServoEvent { * * @param angle the angle to turn the servo to -90 (left) to 90 (right). */ - public SetAngleEvent(double angle) { + public SetAngleInstr(double angle) { this.angle = angle; } @@ -21,7 +21,7 @@ public double angle() { @Override public void actOn(RevServo servo) { if (Math.abs(angle) > 90.0) { - throw new EventUnimplementedException( + throw new IllegalArgumentException( "Angle greater than 90 degrees is not supported for Servos"); } servo.setAngle(angle); diff --git a/src/com/nutrons/framework/controllers/SetEvent.java b/src/com/nutrons/framework/controllers/SetEvent.java deleted file mode 100644 index 8cb8f5f..0000000 --- a/src/com/nutrons/framework/controllers/SetEvent.java +++ /dev/null @@ -1,33 +0,0 @@ -package com.nutrons.framework.controllers; - -public class SetEvent implements ServoEvent { - - private final double value; - - /** - * An event which sets the servo to turn to a certain angle. - * - * @param value the input value to give the servo -1 is full left 1 is full right. - */ - public SetEvent(double value) { - this.value = value; - - } - - public double value() { - return this.value; - - } - - @Override - public void actOn(RevServo servo) { - if (Math.abs(value) > 1.0) { - throw new EventUnimplementedException( - "Value input greater than 1.0 is not supported for this servo"); - - } - servo.set(value); - - } - -} From 6984e8783ed4e9f5be23a0f5ba9dedfae6d40176 Mon Sep 17 00:00:00 2001 From: Josh Young Date: Tue, 21 Feb 2017 20:00:54 -0500 Subject: [PATCH 22/45] Fixed Errors :C --- .../nutrons/framework/controllers/Events.java | 7 ---- .../framework/controllers/RevServo.java | 4 +-- .../framework/controllers/ServoCommand.java | 10 ++++++ .../controllers/ServoController.java | 8 ++--- .../framework/controllers/ServoInstr.java | 11 ++++--- ...etAngleInstr.java => SetAngleCommand.java} | 4 +-- .../framework/controllers/SetCommand.java | 33 +++++++++++++++++++ 7 files changed, 58 insertions(+), 19 deletions(-) create mode 100644 src/com/nutrons/framework/controllers/ServoCommand.java rename src/com/nutrons/framework/controllers/{SetAngleInstr.java => SetAngleCommand.java} (85%) create mode 100644 src/com/nutrons/framework/controllers/SetCommand.java diff --git a/src/com/nutrons/framework/controllers/Events.java b/src/com/nutrons/framework/controllers/Events.java index 9a8360e..bcb474e 100644 --- a/src/com/nutrons/framework/controllers/Events.java +++ b/src/com/nutrons/framework/controllers/Events.java @@ -95,11 +95,4 @@ public static ControllerEvent resetPosition(double position) { return new ResetPositionEvent(position); } - public static ServoInstr setAngle(double angle) { - return new SetAngleInstr(angle); - } - - public static ServoInstr set(double value) { - return new SetInstr(value); - } } diff --git a/src/com/nutrons/framework/controllers/RevServo.java b/src/com/nutrons/framework/controllers/RevServo.java index 430f514..74565cd 100644 --- a/src/com/nutrons/framework/controllers/RevServo.java +++ b/src/com/nutrons/framework/controllers/RevServo.java @@ -18,8 +18,8 @@ public RevServo(int port) { } @Override - public void accept(ServoInstr servoInstr) { - servoInstr.actOn(this); + public void accept(ServoCommand servoCommand) { + servoCommand.actOn(this); } /** diff --git a/src/com/nutrons/framework/controllers/ServoCommand.java b/src/com/nutrons/framework/controllers/ServoCommand.java new file mode 100644 index 0000000..5b1e4b2 --- /dev/null +++ b/src/com/nutrons/framework/controllers/ServoCommand.java @@ -0,0 +1,10 @@ +package com.nutrons.framework.controllers; + +public interface ServoCommand { + + default void actOn(RevServo servo) { + throw new EventUnimplementedException( + servo.getClass().toString(), this.getClass().toString()); + } + +} diff --git a/src/com/nutrons/framework/controllers/ServoController.java b/src/com/nutrons/framework/controllers/ServoController.java index a497de0..ebd8ff4 100644 --- a/src/com/nutrons/framework/controllers/ServoController.java +++ b/src/com/nutrons/framework/controllers/ServoController.java @@ -2,12 +2,12 @@ import io.reactivex.functions.Consumer; -public abstract class ServoController implements Consumer { +public abstract class ServoController implements Consumer { @Override - public abstract void accept(ServoInstr servoInstr); + public abstract void accept(ServoCommand servoCommand); - public void setAngle(double angle) { this.accept(Events.setAngle(angle)); } + public void setAngle(double angle) { this.accept(ServoInstr.setAngle(angle)); } - public void set(double value) { this.accept(Events.set(value)); } + public void set(double value) { this.accept(ServoInstr.set(value)); } } diff --git a/src/com/nutrons/framework/controllers/ServoInstr.java b/src/com/nutrons/framework/controllers/ServoInstr.java index 30eab7a..3c34377 100644 --- a/src/com/nutrons/framework/controllers/ServoInstr.java +++ b/src/com/nutrons/framework/controllers/ServoInstr.java @@ -1,10 +1,13 @@ package com.nutrons.framework.controllers; -public interface ServoInstr { +public class ServoInstr { - default void actOn(RevServo servo) { - throw new EventUnimplementedException( - servo.getClass().toString(), this.getClass().toString()); + public static ServoCommand setAngle(double angle) { + return new SetAngleCommand(angle); + } + + public static ServoCommand set(double value) { + return new SetCommand(value); } } diff --git a/src/com/nutrons/framework/controllers/SetAngleInstr.java b/src/com/nutrons/framework/controllers/SetAngleCommand.java similarity index 85% rename from src/com/nutrons/framework/controllers/SetAngleInstr.java rename to src/com/nutrons/framework/controllers/SetAngleCommand.java index 0d99807..38206a8 100644 --- a/src/com/nutrons/framework/controllers/SetAngleInstr.java +++ b/src/com/nutrons/framework/controllers/SetAngleCommand.java @@ -1,6 +1,6 @@ package com.nutrons.framework.controllers; -public class SetAngleInstr implements ServoInstr { +public class SetAngleCommand implements ServoCommand { private final double angle; @@ -9,7 +9,7 @@ public class SetAngleInstr implements ServoInstr { * * @param angle the angle to turn the servo to -90 (left) to 90 (right). */ - public SetAngleInstr(double angle) { + public SetAngleCommand(double angle) { this.angle = angle; } diff --git a/src/com/nutrons/framework/controllers/SetCommand.java b/src/com/nutrons/framework/controllers/SetCommand.java new file mode 100644 index 0000000..8a893eb --- /dev/null +++ b/src/com/nutrons/framework/controllers/SetCommand.java @@ -0,0 +1,33 @@ +package com.nutrons.framework.controllers; + +public class SetCommand implements ServoCommand { + + private final double value; + + /** + * An event which sets the servo to turn to a certain angle. + * + * @param value the input value to give the servo -1 is full left 1 is full right. + */ + public SetCommand(double value) { + this.value = value; + + } + + public double value() { + return this.value; + + } + + @Override + public void actOn(RevServo servo) { + if (Math.abs(value) > 1.0) { + throw new IllegalArgumentException( + "Value input greater than 1.0 is not supported for this servo"); + + } + servo.set(value); + + } + +} From 1837abbcafb1d7b8337da7462847829d34611020 Mon Sep 17 00:00:00 2001 From: Josh Young Date: Tue, 21 Feb 2017 23:15:06 -0500 Subject: [PATCH 23/45] Fixed dumb style errors --- src/com/nutrons/framework/StreamManager.java | 13 ++++--- .../nutrons/framework/commands/Command.java | 35 ++++++++++++------ .../framework/commands/CommandWorkUnit.java | 1 + .../framework/commands/ParallelCommand.java | 1 + .../framework/commands/SerialCommand.java | 1 + .../nutrons/framework/controllers/Events.java | 1 + .../framework/controllers/FollowEvent.java | 1 + .../framework/controllers/RevServo.java | 3 ++ .../controllers/ServoController.java | 8 +++-- .../nutrons/framework/controllers/Talon.java | 6 ++-- src/com/nutrons/framework/inputs/Serial.java | 4 +-- .../subsystems/WpiSmartDashboard.java | 5 ++- .../nutrons/framework/util/FlowOperators.java | 36 +++++++++++-------- .../framework/test/MultiCommandTest.java | 7 ++-- .../nutrons/framework/test/TestCommand.java | 17 +++++---- 15 files changed, 88 insertions(+), 51 deletions(-) diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index 8c91302..891cccc 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -1,5 +1,9 @@ package com.nutrons.framework; +import static com.nutrons.framework.util.CompMode.AUTO; +import static com.nutrons.framework.util.CompMode.TELE; +import static io.reactivex.Flowable.combineLatest; + import com.nutrons.framework.commands.Command; import com.nutrons.framework.util.CompMode; import io.reactivex.Flowable; @@ -10,9 +14,7 @@ import java.util.List; import java.util.function.Supplier; -import static com.nutrons.framework.util.CompMode.AUTO; -import static com.nutrons.framework.util.CompMode.TELE; -import static io.reactivex.Flowable.combineLatest; + /** * This class sets up the I/O factories and initializes subsystems. @@ -38,7 +40,7 @@ public StreamManager(Robot robot) { * Subclasses should register subsystems in their constructor. * * @param enabled a Flowable of booleans, representing changes in the enabled state of the robot - * @param mode a Flowable of CompModes, representing changes in the competition game mode + * @param mode a Flowable of CompModes, representing changes in the competition game mode */ public StreamManager(Flowable enabled, Flowable mode) { this.subsystems = new ArrayList<>(); @@ -50,7 +52,8 @@ public StreamManager(Flowable enabled, Flowable mode) { * Called to by the bootstrapper to subscribe subsystem streams, * and start the competition loop. */ - public final void startCompetition(Supplier autoSupplier, Supplier teleopSupplier) { + public final void startCompetition(Supplier autoSupplier, + Supplier teleopSupplier) { Observable.fromIterable(this.subsystems).blockingSubscribe(x -> { System.out.println("registering " + x.getClass().getName()); x.registerSubscriptions(); diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 38b98dc..d8b70e0 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -1,16 +1,15 @@ package com.nutrons.framework.commands; +import static com.nutrons.framework.util.FlowOperators.toFlow; + import io.reactivex.Flowable; import io.reactivex.Observable; import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.schedulers.Schedulers; -import org.reactivestreams.Publisher; - import java.util.concurrent.TimeUnit; import java.util.function.Supplier; - -import static com.nutrons.framework.util.FlowOperators.toFlow; +import org.reactivestreams.Publisher; public class Command implements CommandWorkUnit { @@ -81,13 +80,20 @@ public static Command fromSwitch(Publisher commandStr return new Command(x -> Flowable.defer(() -> Flowable.switchOnNext(Flowable.fromPublisher(commandStream).map(y -> y.execute(x)) .subscribeOn(Schedulers.io()))).scan((a, b) -> { - a.run(); - return b; - })); + a.run(); + return b; + })); } + /** + * Adds a command that will terminate the current command. + * @param terminator Terminator command ou wish to add. + * @return teriminatable command. + */ public Command addFinalTerminator(Terminator terminator) { - return Command.just(x -> this.source.execute(x).flatMap(y -> Flowable.just(y, terminator)).subscribeOn(Schedulers.io())); + return Command.just( + x -> this.source.execute(x).flatMap(y -> Flowable.just(y, terminator)) + .subscribeOn(Schedulers.io())); } /** @@ -120,8 +126,8 @@ public Command terminable(Publisher terminator) { /** * Copies this command into one which will end when terminator emits an item. * - * @param terminatesAtEnd if true, the command will terminate only when the end is reached, - * if false, the command may terminate before the end is reached. + * @param terminatesAtEnd if true, the command will terminate only when the end is reached, if + * false, the command may terminate before the end is reached. */ public Command endsWhen(Publisher terminator, boolean terminatesAtEnd) { return Command.just(x -> { @@ -137,7 +143,8 @@ public Command endsWhen(Publisher terminator, boolean terminatesAtEnd) { * will only complete once endCondition returns true. */ public Command until(Supplier endCondition) { - ConnectableFlowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x).onBackpressureDrop().publish(); + ConnectableFlowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x) + .onBackpressureDrop().publish(); terminator.connect(); return this.terminable(terminator); } @@ -163,6 +170,12 @@ public Command delayFinish(long delay, TimeUnit unit) { return this.terminable(Flowable.timer(delay, unit)); } + /** + * Kills a command after a given time and unit + * @param delay a number in relation to a unit 1000 = 1 ms if unit is ms. + * @param unit unit you wish to count in. + * @return a command that will terminate after a given time. + */ public Command killAfter(long delay, TimeUnit unit) { return Command.just(x -> { Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(x); diff --git a/src/com/nutrons/framework/commands/CommandWorkUnit.java b/src/com/nutrons/framework/commands/CommandWorkUnit.java index 8458be0..c71ddd1 100644 --- a/src/com/nutrons/framework/commands/CommandWorkUnit.java +++ b/src/com/nutrons/framework/commands/CommandWorkUnit.java @@ -3,5 +3,6 @@ import io.reactivex.Flowable; public interface CommandWorkUnit { + Flowable execute(boolean selfTerminating); } diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index df227ad..53b05a6 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -4,6 +4,7 @@ import io.reactivex.schedulers.Schedulers; public class ParallelCommand implements CommandWorkUnit { + private final Flowable commands; ParallelCommand(CommandWorkUnit... commands) { diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index 0914471..ac968b5 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -4,6 +4,7 @@ import io.reactivex.schedulers.Schedulers; public class SerialCommand implements CommandWorkUnit { + private final Flowable commands; SerialCommand(CommandWorkUnit... commands) { diff --git a/src/com/nutrons/framework/controllers/Events.java b/src/com/nutrons/framework/controllers/Events.java index bcb474e..63612b0 100644 --- a/src/com/nutrons/framework/controllers/Events.java +++ b/src/com/nutrons/framework/controllers/Events.java @@ -35,6 +35,7 @@ public static ControllerEvent pid(double pval, double ival, double dval, double /** * Combines actions that are preformed on the talon. + * * @return combined event */ public static ControllerEvent combine(ControllerEvent... events) { diff --git a/src/com/nutrons/framework/controllers/FollowEvent.java b/src/com/nutrons/framework/controllers/FollowEvent.java index a46dc09..7d717de 100644 --- a/src/com/nutrons/framework/controllers/FollowEvent.java +++ b/src/com/nutrons/framework/controllers/FollowEvent.java @@ -6,6 +6,7 @@ public class FollowEvent implements ControllerEvent { /** * Sets a LoopSpeedController to follow another LoopSpeedController + * * @param targetToFollow is a LoopSpeedController which will be followed. */ public FollowEvent(LoopSpeedController targetToFollow) { diff --git a/src/com/nutrons/framework/controllers/RevServo.java b/src/com/nutrons/framework/controllers/RevServo.java index 74565cd..ef40826 100644 --- a/src/com/nutrons/framework/controllers/RevServo.java +++ b/src/com/nutrons/framework/controllers/RevServo.java @@ -11,6 +11,7 @@ public class RevServo extends ServoController { /** * Makes a RevServo object at the given port. + * * @param port Port of RevServo */ public RevServo(int port) { @@ -24,6 +25,7 @@ public void accept(ServoCommand servoCommand) { /** * Sets the Servo to a given angle ranging from -90 to 90 left and right reflectively. + * * @param angle given angle to turn to. */ public void setAngle(double angle) { @@ -32,6 +34,7 @@ public void setAngle(double angle) { /** * Set the motor to a position given a value 0.0 is full left 1.0 is full right. + * * @param value The value to turn the servo to. */ public void set(double value) { diff --git a/src/com/nutrons/framework/controllers/ServoController.java b/src/com/nutrons/framework/controllers/ServoController.java index ebd8ff4..e079faa 100644 --- a/src/com/nutrons/framework/controllers/ServoController.java +++ b/src/com/nutrons/framework/controllers/ServoController.java @@ -7,7 +7,11 @@ public abstract class ServoController implements Consumer { @Override public abstract void accept(ServoCommand servoCommand); - public void setAngle(double angle) { this.accept(ServoInstr.setAngle(angle)); } + public void setAngle(double angle) { + this.accept(ServoInstr.setAngle(angle)); + } - public void set(double value) { this.accept(ServoInstr.set(value)); } + public void set(double value) { + this.accept(ServoInstr.set(value)); + } } diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index 5babd71..c6c4585 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -1,11 +1,11 @@ package com.nutrons.framework.controllers; +import static com.nutrons.framework.util.FlowOperators.toFlow; + import com.ctre.CANTalon; import io.reactivex.Flowable; - import java.security.InvalidParameterException; -import static com.nutrons.framework.util.FlowOperators.toFlow; public class Talon extends LoopSpeedController { @@ -133,7 +133,7 @@ void resetPositionTo(double position) { @Override - public boolean fwdLimitSwitchClosed(){ + public boolean fwdLimitSwitchClosed() { return this.talon.isFwdLimitSwitchClosed(); } diff --git a/src/com/nutrons/framework/inputs/Serial.java b/src/com/nutrons/framework/inputs/Serial.java index e872c5e..970450b 100644 --- a/src/com/nutrons/framework/inputs/Serial.java +++ b/src/com/nutrons/framework/inputs/Serial.java @@ -41,7 +41,7 @@ public Serial(int bufferSize, int packetLength) { * @param bufferSize represents how many bytes to cache unread before clearing buffer * @param packetLength represents the length of each read from the buffer * @param terminationCharacter allows users to set a custom termination character - default is the - * newline character '\n' + * newline character '\n' */ public Serial(SerialPort.Port port, int bufferSize, @@ -56,7 +56,7 @@ public Serial(SerialPort.Port port, * @param bufferSize represents how many bytes to cache unread before clearing buffer * @param packetLength represents the length of each read from the buffer * @param terminationCharacter allows users to set a custom termination character - default is the - * newline character '\n' + * newline character '\n' * @param baudrate See Symbol Rate */ public Serial(int baudrate, diff --git a/src/com/nutrons/framework/subsystems/WpiSmartDashboard.java b/src/com/nutrons/framework/subsystems/WpiSmartDashboard.java index 6675229..44f3b5c 100644 --- a/src/com/nutrons/framework/subsystems/WpiSmartDashboard.java +++ b/src/com/nutrons/framework/subsystems/WpiSmartDashboard.java @@ -24,6 +24,7 @@ public WpiSmartDashboard() { /** * Takes a double and passes it to the Smart Dashboard. + * * @param key decimal passed to the Smartdash. */ public Consumer getTextFieldDouble(String key) { @@ -39,7 +40,8 @@ public Consumer getTextFieldDouble(String key) { /** * Prints string to the Smart Dashborad. - * @param key A specific String. + * + * @param key A specific String. */ public Consumer getTextFieldString(String key) { if (!stringFields.containsKey(key)) { @@ -54,6 +56,7 @@ public Consumer getTextFieldString(String key) { /** * Prints Output of a Boolean to the Smart Dashboard. + * * @param key boolean statement. */ public Consumer getTextFieldBoolean(String key) { diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index c5e4e75..5a1ebf3 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -1,18 +1,19 @@ package com.nutrons.framework.util; +import static java.lang.Math.abs; +import static java.lang.Math.max; +import static java.lang.Math.min; + import io.reactivex.Flowable; import io.reactivex.FlowableTransformer; import io.reactivex.disposables.CompositeDisposable; import io.reactivex.disposables.Disposable; import io.reactivex.functions.Function; import io.reactivex.schedulers.Schedulers; - import java.util.concurrent.TimeUnit; import java.util.function.Supplier; -import static java.lang.Math.abs; -import static java.lang.Math.max; -import static java.lang.Math.min; + public class FlowOperators { @@ -20,10 +21,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(); @@ -53,7 +54,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); } @@ -62,7 +63,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; } @@ -74,9 +75,9 @@ 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 error -> { Flowable errorP = error.map(x -> x * proportional); Flowable errorI = error.buffer(integralBuffer, 1) @@ -95,15 +96,20 @@ public static Function limitWithin(double minimum, double maximu return x -> max(min(x, maximum), minimum); } - public static T printId(T t) { - System.out.println(t); - return t; + public static T printId(T flowables) { + System.out.println(flowables); + return flowables; } + /** + * Combines disposable given to this method in order to return them all at once. + * @param disposables the disposables you desire to combine + * @return combined disposables. + */ public static Disposable combineDisposable(Disposable... disposables) { CompositeDisposable cd = new CompositeDisposable(); Flowable.fromArray(disposables).blockingSubscribe(cd::add); return cd; } - + } diff --git a/test/com/nutrons/framework/test/MultiCommandTest.java b/test/com/nutrons/framework/test/MultiCommandTest.java index 510e164..49cb3fd 100644 --- a/test/com/nutrons/framework/test/MultiCommandTest.java +++ b/test/com/nutrons/framework/test/MultiCommandTest.java @@ -1,15 +1,16 @@ package com.nutrons.framework.test; +import static com.nutrons.framework.commands.Command.parallel; +import static junit.framework.TestCase.assertTrue; + import com.nutrons.framework.commands.Command; import com.nutrons.framework.commands.TerminatorWrapper; import io.reactivex.Flowable; +import java.util.concurrent.TimeUnit; import org.junit.Before; import org.junit.Test; -import java.util.concurrent.TimeUnit; -import static com.nutrons.framework.commands.Command.parallel; -import static junit.framework.TestCase.assertTrue; public class MultiCommandTest { private Command delay; diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index bc33a49..f52473d 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -1,19 +1,18 @@ package com.nutrons.framework.test; +import static com.nutrons.framework.commands.Command.parallel; +import static com.nutrons.framework.commands.Command.serial; +import static junit.framework.TestCase.assertTrue; + import com.nutrons.framework.commands.Command; import com.nutrons.framework.commands.Terminator; import io.reactivex.Flowable; import io.reactivex.processors.PublishProcessor; import io.reactivex.schedulers.Schedulers; +import java.util.concurrent.TimeUnit; import org.junit.Before; import org.junit.Test; -import java.util.concurrent.TimeUnit; - -import static com.nutrons.framework.commands.Command.parallel; -import static com.nutrons.framework.commands.Command.serial; -import static junit.framework.TestCase.assertTrue; - public class TestCommand { private Command delay; @@ -72,9 +71,8 @@ public void inParallelTimed() { public void testTerminable() throws InterruptedException { final long start = System.currentTimeMillis(); PublishProcessor pp = PublishProcessor.create(); - Flowable td = serial(delay, delay, delay, delay) - .terminable(pp).execute(true); Thread.sleep(3000); + Flowable td = serial(delay, delay, delay, delay).terminable(pp).execute(true); pp.onNext(new Object()); pp.onComplete(); waitForCommand(td); @@ -86,7 +84,8 @@ public void testUntil() throws InterruptedException { int[] record = new int[2]; assertTrue(record[0] == 0); long start = System.currentTimeMillis(); - Flowable td = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1).execute(true); + Flowable td = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1) + .execute(true); Flowable.timer(1, TimeUnit.SECONDS).subscribeOn(Schedulers.io()).subscribe(x -> record[1] = 1); waitForCommand(td); assertTrue(System.currentTimeMillis() - 1000 > start); From a1e994853a8875c0476c6f824f4fc443c8734650 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Fri, 24 Feb 2017 08:53:28 -0500 Subject: [PATCH 24/45] added toFlowFast --- src/com/nutrons/framework/util/FlowOperators.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index ecd9454..c7c78b5 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -39,7 +39,7 @@ public static Flowable toFlow(Supplier supplier) { } public static Flowable toFlowFast(Supplier supplier) { - return toFlow(supplier, 100, TimeUnit.MILLISECONDS); + return toFlow(supplier, 50, TimeUnit.MILLISECONDS); } /** From 27c02412e1ce864d784d9f58d7dc1b8cc9df9c3e Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Sat, 25 Feb 2017 14:22:10 -0500 Subject: [PATCH 25/45] updated fromSwitch to work, and added / modified tests --- .../nutrons/framework/commands/Command.java | 24 ++--- .../framework/commands/CommandWorkUnit.java | 2 +- .../commands/FlattenedTerminator.java | 6 +- .../framework/test/MultiCommandTest.java | 28 +----- .../nutrons/framework/test/TestCommand.java | 17 ++-- .../framework/test/TestSwitchCommand.java | 94 +++++++++++++++++++ 6 files changed, 121 insertions(+), 50 deletions(-) create mode 100644 test/com/nutrons/framework/test/TestSwitchCommand.java diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 38b98dc..3be7636 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -1,17 +1,16 @@ package com.nutrons.framework.commands; +import static com.nutrons.framework.util.FlowOperators.toFlow; + import io.reactivex.Flowable; import io.reactivex.Observable; import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.schedulers.Schedulers; import org.reactivestreams.Publisher; - import java.util.concurrent.TimeUnit; import java.util.function.Supplier; -import static com.nutrons.framework.util.FlowOperators.toFlow; - public class Command implements CommandWorkUnit { // emptyPulse sends items on an interval, and is used in the 'until' and 'when' methods @@ -79,11 +78,12 @@ public static Command parallel(Command... commands) { */ public static Command fromSwitch(Publisher commandStream) { return new Command(x -> Flowable.defer(() -> - Flowable.switchOnNext(Flowable.fromPublisher(commandStream).map(y -> y.execute(x)) - .subscribeOn(Schedulers.io()))).scan((a, b) -> { - a.run(); - return b; - })); + Flowable.fromPublisher(commandStream) + .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(x))) + .subscribeOn(Schedulers.io())).scan((a, b) -> { + a.run(); + return b; + }))); } public Command addFinalTerminator(Terminator terminator) { @@ -125,7 +125,7 @@ public Command terminable(Publisher terminator) { */ public Command endsWhen(Publisher terminator, boolean terminatesAtEnd) { return Command.just(x -> { - Flowable sourceTerminator = this.execute(terminatesAtEnd); + Flowable sourceTerminator = this.execute(terminatesAtEnd); Terminator multi = FlattenedTerminator.from(sourceTerminator); return Flowable.defer(() -> Flowable.never().takeUntil(terminator) .mergeWith(Flowable.just(multi::run))); @@ -165,14 +165,14 @@ public Command delayFinish(long delay, TimeUnit unit) { public Command killAfter(long delay, TimeUnit unit) { return Command.just(x -> { - Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(x); + Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(x); return terms; }); } @Override - public Flowable execute(boolean selfTerminating) { - Flowable terms = source.execute(selfTerminating).subscribeOn(Schedulers.io()); + public Flowable execute(boolean selfTerminating) { + Flowable terms = source.execute(selfTerminating).subscribeOn(Schedulers.io()); if (selfTerminating) { terms.toList().subscribe(x -> Observable.fromIterable(x).blockingSubscribe(Terminator::run)); } diff --git a/src/com/nutrons/framework/commands/CommandWorkUnit.java b/src/com/nutrons/framework/commands/CommandWorkUnit.java index 8458be0..34c83b0 100644 --- a/src/com/nutrons/framework/commands/CommandWorkUnit.java +++ b/src/com/nutrons/framework/commands/CommandWorkUnit.java @@ -3,5 +3,5 @@ import io.reactivex.Flowable; public interface CommandWorkUnit { - Flowable execute(boolean selfTerminating); + Flowable execute(boolean selfTerminating); } diff --git a/src/com/nutrons/framework/commands/FlattenedTerminator.java b/src/com/nutrons/framework/commands/FlattenedTerminator.java index 33779b4..769603c 100644 --- a/src/com/nutrons/framework/commands/FlattenedTerminator.java +++ b/src/com/nutrons/framework/commands/FlattenedTerminator.java @@ -10,9 +10,9 @@ class FlattenedTerminator implements Terminator { private final AtomicBoolean lock; private final ArrayList terminators; - private final Flowable terminatorStream; + private final Flowable terminatorStream; - private FlattenedTerminator(Flowable terminators) { + private FlattenedTerminator(Flowable terminators) { this.lock = new AtomicBoolean(false); this.terminators = new ArrayList<>(); this.terminatorStream = terminators; @@ -29,7 +29,7 @@ private FlattenedTerminator(Flowable terminators) { }); } - static FlattenedTerminator from(Flowable terminators) { + static FlattenedTerminator from(Flowable terminators) { return new FlattenedTerminator(terminators); } diff --git a/test/com/nutrons/framework/test/MultiCommandTest.java b/test/com/nutrons/framework/test/MultiCommandTest.java index 510e164..45f14b0 100644 --- a/test/com/nutrons/framework/test/MultiCommandTest.java +++ b/test/com/nutrons/framework/test/MultiCommandTest.java @@ -1,16 +1,14 @@ package com.nutrons.framework.test; +import static com.nutrons.framework.commands.Command.parallel; +import static junit.framework.TestCase.assertTrue; + import com.nutrons.framework.commands.Command; -import com.nutrons.framework.commands.TerminatorWrapper; import io.reactivex.Flowable; import org.junit.Before; import org.junit.Test; - import java.util.concurrent.TimeUnit; -import static com.nutrons.framework.commands.Command.parallel; -import static junit.framework.TestCase.assertTrue; - public class MultiCommandTest { private Command delay; @@ -41,24 +39,4 @@ public void testOneThenAnother() throws InterruptedException { Thread.sleep(2000); assertTrue(record[0] == 0); } - - @Test - public void testSwitch() throws InterruptedException { - int[] record = new int[1]; - record[0] = 0; - Command inc = Command.just(x -> { - synchronized (record) { - record[0] += 1; - } - return Flowable.just(new TerminatorWrapper(() -> { - synchronized (record) { - record[0] -= 1; - } - })); - }); - Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) - .execute(true).blockingSubscribe(); - Thread.sleep(2000); - assertTrue(record[0] == 0); - } } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index bc33a49..b1deab4 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -1,5 +1,9 @@ package com.nutrons.framework.test; +import static com.nutrons.framework.commands.Command.parallel; +import static com.nutrons.framework.commands.Command.serial; +import static junit.framework.TestCase.assertTrue; + import com.nutrons.framework.commands.Command; import com.nutrons.framework.commands.Terminator; import io.reactivex.Flowable; @@ -7,18 +11,13 @@ import io.reactivex.schedulers.Schedulers; import org.junit.Before; import org.junit.Test; - import java.util.concurrent.TimeUnit; -import static com.nutrons.framework.commands.Command.parallel; -import static com.nutrons.framework.commands.Command.serial; -import static junit.framework.TestCase.assertTrue; - public class TestCommand { private Command delay; - static void waitForCommand(Flowable commandExecution) { + static void waitForCommand(Flowable commandExecution) { commandExecution.blockingSubscribe(); } @@ -72,7 +71,7 @@ public void inParallelTimed() { public void testTerminable() throws InterruptedException { final long start = System.currentTimeMillis(); PublishProcessor pp = PublishProcessor.create(); - Flowable td = serial(delay, delay, delay, delay) + Flowable td = serial(delay, delay, delay, delay) .terminable(pp).execute(true); Thread.sleep(3000); pp.onNext(new Object()); @@ -86,7 +85,7 @@ public void testUntil() throws InterruptedException { int[] record = new int[2]; assertTrue(record[0] == 0); long start = System.currentTimeMillis(); - Flowable td = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1).execute(true); + Flowable td = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1).execute(true); Flowable.timer(1, TimeUnit.SECONDS).subscribeOn(Schedulers.io()).subscribe(x -> record[1] = 1); waitForCommand(td); assertTrue(System.currentTimeMillis() - 1000 > start); @@ -109,7 +108,7 @@ public void testStartable() { public void testWhen() throws InterruptedException { int[] record = new int[2]; assertTrue(record[0] == 0); - final Flowable td = Command.fromAction(() -> record[0] = 1) + final Flowable td = Command.fromAction(() -> record[0] = 1) .when(() -> record[1] == 1) .execute(true); Thread.sleep(1000); diff --git a/test/com/nutrons/framework/test/TestSwitchCommand.java b/test/com/nutrons/framework/test/TestSwitchCommand.java new file mode 100644 index 0000000..2040a36 --- /dev/null +++ b/test/com/nutrons/framework/test/TestSwitchCommand.java @@ -0,0 +1,94 @@ +package com.nutrons.framework.test; + +import static junit.framework.TestCase.assertFalse; +import static junit.framework.TestCase.assertTrue; + +import com.nutrons.framework.commands.Command; +import com.nutrons.framework.commands.TerminatorWrapper; +import io.reactivex.Flowable; +import org.junit.Test; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.TimeUnit; + +public class TestSwitchCommand { + + @Test + public void testSwitchStart() throws InterruptedException { + List list = new ArrayList<>(); + Flowable commandStream = Flowable.interval(100, TimeUnit.MILLISECONDS) + .map(x -> putNumber(list, x)).take(5); + Command.fromSwitch(commandStream).execute(true); + Thread.sleep(300); + assertFalse(list.contains(4)); + Thread.sleep(500); + for (long i = 0; i < 5; i++) { + assertTrue(list.contains(i)); + } + } + + private Command putNumber(List list, long number) { + return Command.fromAction(() -> list.add(number)); + } + + + @Test + public void testSwitchStartAndStop() throws InterruptedException { + int[] record = new int[1]; + record[0] = 0; + Command inc = Command.just(x -> { + synchronized (record) { + record[0] += 1; + } + return Flowable.just(new TerminatorWrapper(() -> { + synchronized (record) { + record[0] -= 1; + } + })); + }); + Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) + .execute(true).blockingSubscribe(); + Thread.sleep(2000); + assertTrue(record[0] == 0); + } + + @Test + public void testSwitchTerminateRealtime() throws InterruptedException { + int[] record = new int[1]; + long start = System.currentTimeMillis(); + record[0] = 0; + Command inc = Command.just(x -> Flowable.just(new TerminatorWrapper(() -> { + synchronized (record) { + record[0] += 1; + } + }))); + Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) + .execute(true); + assertTrue(System.currentTimeMillis() - start < 1000); + Thread.sleep(2000); + assertTrue(record[0] < 5); + assertTrue(record[0] > 0); + } + + @Test + public void testSwitchNotTerminating() throws InterruptedException { + Command doesntFinish = Command.just(x -> Flowable.just(() -> assertTrue(false))); + doesntFinish.execute(false); + Thread.sleep(1000); + Command justOne = Command.fromSwitch(Flowable.never() + .mergeWith(Flowable.just(doesntFinish))); + justOne.execute(false); + Thread.sleep(1000); + justOne.execute(true); + Thread.sleep(1000); + } + + @Test(expected = RuntimeException.class) + public void testSwitchTerminatesOnNext() { + Command doesntFinish = Command.just(x -> Flowable.just(() -> { + throw new RuntimeException(); + })); + Command two = Command.fromSwitch(Flowable.just(doesntFinish, doesntFinish)); + two.execute(false).blockingSubscribe(); + } +} From e44bf5e28246a7b89ca5c2f807439b67dacdd2d2 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Sat, 25 Feb 2017 14:31:33 -0500 Subject: [PATCH 26/45] fixed some style --- .../nutrons/framework/commands/Command.java | 26 ++++++++++++------- .../framework/test/MultiCommandTest.java | 2 +- .../nutrons/framework/test/TestCommand.java | 7 ++--- .../framework/test/TestSwitchCommand.java | 2 +- 4 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 3be7636..7cd1566 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -7,9 +7,9 @@ import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.schedulers.Schedulers; -import org.reactivestreams.Publisher; import java.util.concurrent.TimeUnit; import java.util.function.Supplier; +import org.reactivestreams.Publisher; public class Command implements CommandWorkUnit { @@ -80,14 +80,16 @@ public static Command fromSwitch(Publisher commandStr return new Command(x -> Flowable.defer(() -> Flowable.fromPublisher(commandStream) .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(x))) - .subscribeOn(Schedulers.io())).scan((a, b) -> { - a.run(); - return b; - }))); + .subscribeOn(Schedulers.io())) + .scan((a, b) -> { + a.run(); + return b; + }))); } public Command addFinalTerminator(Terminator terminator) { - return Command.just(x -> this.source.execute(x).flatMap(y -> Flowable.just(y, terminator)).subscribeOn(Schedulers.io())); + return Command.just(x -> this.source.execute(x) + .flatMap(y -> Flowable.just(y, terminator)).subscribeOn(Schedulers.io())); } /** @@ -137,7 +139,8 @@ public Command endsWhen(Publisher terminator, boolean terminatesAtEnd) { * will only complete once endCondition returns true. */ public Command until(Supplier endCondition) { - ConnectableFlowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x).onBackpressureDrop().publish(); + ConnectableFlowable terminator = emptyPulse.map(x -> endCondition.get()).filter(x -> x) + .onBackpressureDrop().publish(); terminator.connect(); return this.terminable(terminator); } @@ -163,16 +166,21 @@ public Command delayFinish(long delay, TimeUnit unit) { return this.terminable(Flowable.timer(delay, unit)); } + /** + * End and terminate this command only after the specified time has passed. + */ public Command killAfter(long delay, TimeUnit unit) { return Command.just(x -> { - Flowable terms = this.terminable(Flowable.timer(delay, unit)).execute(x); + Flowable terms = this.terminable(Flowable.timer(delay, unit)) + .execute(x); return terms; }); } @Override public Flowable execute(boolean selfTerminating) { - Flowable terms = source.execute(selfTerminating).subscribeOn(Schedulers.io()); + Flowable terms = source.execute(selfTerminating) + .subscribeOn(Schedulers.io()); if (selfTerminating) { terms.toList().subscribe(x -> Observable.fromIterable(x).blockingSubscribe(Terminator::run)); } diff --git a/test/com/nutrons/framework/test/MultiCommandTest.java b/test/com/nutrons/framework/test/MultiCommandTest.java index 45f14b0..520dd37 100644 --- a/test/com/nutrons/framework/test/MultiCommandTest.java +++ b/test/com/nutrons/framework/test/MultiCommandTest.java @@ -5,9 +5,9 @@ import com.nutrons.framework.commands.Command; import io.reactivex.Flowable; +import java.util.concurrent.TimeUnit; import org.junit.Before; import org.junit.Test; -import java.util.concurrent.TimeUnit; public class MultiCommandTest { private Command delay; diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index b1deab4..abbc0c2 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -9,9 +9,9 @@ import io.reactivex.Flowable; import io.reactivex.processors.PublishProcessor; import io.reactivex.schedulers.Schedulers; +import java.util.concurrent.TimeUnit; import org.junit.Before; import org.junit.Test; -import java.util.concurrent.TimeUnit; public class TestCommand { @@ -71,7 +71,7 @@ public void inParallelTimed() { public void testTerminable() throws InterruptedException { final long start = System.currentTimeMillis(); PublishProcessor pp = PublishProcessor.create(); - Flowable td = serial(delay, delay, delay, delay) + final Flowable td = serial(delay, delay, delay, delay) .terminable(pp).execute(true); Thread.sleep(3000); pp.onNext(new Object()); @@ -85,7 +85,8 @@ public void testUntil() throws InterruptedException { int[] record = new int[2]; assertTrue(record[0] == 0); long start = System.currentTimeMillis(); - Flowable td = Command.fromAction(() -> record[0] = 1).until(() -> record[1] == 1).execute(true); + Flowable td = Command.fromAction(() -> record[0] = 1) + .until(() -> record[1] == 1).execute(true); Flowable.timer(1, TimeUnit.SECONDS).subscribeOn(Schedulers.io()).subscribe(x -> record[1] = 1); waitForCommand(td); assertTrue(System.currentTimeMillis() - 1000 > start); diff --git a/test/com/nutrons/framework/test/TestSwitchCommand.java b/test/com/nutrons/framework/test/TestSwitchCommand.java index 2040a36..1df5d45 100644 --- a/test/com/nutrons/framework/test/TestSwitchCommand.java +++ b/test/com/nutrons/framework/test/TestSwitchCommand.java @@ -6,10 +6,10 @@ import com.nutrons.framework.commands.Command; import com.nutrons.framework.commands.TerminatorWrapper; import io.reactivex.Flowable; -import org.junit.Test; import java.util.ArrayList; import java.util.List; import java.util.concurrent.TimeUnit; +import org.junit.Test; public class TestSwitchCommand { From b0655e615f83faccba391e69ddbbf4145171dc14 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Mon, 27 Feb 2017 18:40:21 -0500 Subject: [PATCH 27/45] moved libKudos254 to nu-17 Framework' --- docs/google_checks.xml | 12 +- src/com/nutrons/libKudos254/Rotation2d.java | 113 --------- .../nutrons/libKudos254/vision/AdbBridge.java | 80 ------ .../libKudos254/vision/CrashTracker.java | 66 ----- .../vision/CrashTrackingRunnable.java | 19 -- .../libKudos254/vision/TargetInfo.java | 28 --- .../libKudos254/vision/VisionServer.java | 234 ------------------ .../libKudos254/vision/VisionServerTest.java | 29 --- .../libKudos254/vision/VisionUpdate.java | 102 -------- .../vision/VisionUpdateReceiver.java | 11 - .../vision/messages/HeartbeatMessage.java | 27 -- .../vision/messages/OffWireMessage.java | 41 --- .../vision/messages/SetCameraModeMessage.java | 35 --- .../vision/messages/VisionMessage.java | 21 -- 14 files changed, 6 insertions(+), 812 deletions(-) delete mode 100644 src/com/nutrons/libKudos254/Rotation2d.java delete mode 100644 src/com/nutrons/libKudos254/vision/AdbBridge.java delete mode 100644 src/com/nutrons/libKudos254/vision/CrashTracker.java delete mode 100644 src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java delete mode 100644 src/com/nutrons/libKudos254/vision/TargetInfo.java delete mode 100644 src/com/nutrons/libKudos254/vision/VisionServer.java delete mode 100644 src/com/nutrons/libKudos254/vision/VisionServerTest.java delete mode 100644 src/com/nutrons/libKudos254/vision/VisionUpdate.java delete mode 100644 src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java delete mode 100644 src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java delete mode 100644 src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java delete mode 100644 src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java delete mode 100644 src/com/nutrons/libKudos254/vision/messages/VisionMessage.java diff --git a/docs/google_checks.xml b/docs/google_checks.xml index 5c47285..20b86ba 100644 --- a/docs/google_checks.xml +++ b/docs/google_checks.xml @@ -108,32 +108,32 @@ value="Type name ''{0}'' must match pattern ''{1}''."/> - + - + - + - + - + @@ -221,7 +221,7 @@ - + diff --git a/src/com/nutrons/libKudos254/Rotation2d.java b/src/com/nutrons/libKudos254/Rotation2d.java deleted file mode 100644 index f346dba..0000000 --- a/src/com/nutrons/libKudos254/Rotation2d.java +++ /dev/null @@ -1,113 +0,0 @@ -package com.nutrons.libKudos254; - -import java.text.DecimalFormat; - -/** - * A rotation in a 2d coordinate frame represented a point on the unit circle - * (cosine and sine). - * - * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) - */ -public class Rotation2d { - protected static final double kEpsilon = 1E-9; - - protected double cos_angle_; - protected double sin_angle_; - - public Rotation2d() { - this(1, 0, false); - } - - public Rotation2d(double x, double y, boolean normalize) { - cos_angle_ = x; - sin_angle_ = y; - if (normalize) { - normalize(); - } - } - - public Rotation2d(Rotation2d other) { - cos_angle_ = other.cos_angle_; - sin_angle_ = other.sin_angle_; - } - - public static Rotation2d fromRadians(double angle_radians) { - return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false); - } - - public static Rotation2d fromDegrees(double angle_degrees) { - return fromRadians(Math.toRadians(angle_degrees)); - } - - /** - * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this - * object we might accumulate rounding errors. Normalizing forces us to - * re-scale the sin and cos to reset rounding errors. - */ - public void normalize() { - double magnitude = Math.hypot(cos_angle_, sin_angle_); - if (magnitude > kEpsilon) { - sin_angle_ /= magnitude; - cos_angle_ /= magnitude; - } else { - sin_angle_ = 0; - cos_angle_ = 1; - } - } - - public double cos() { - return cos_angle_; - } - - public double sin() { - return sin_angle_; - } - - public double tan() { - if (cos_angle_ < kEpsilon) { - if (sin_angle_ >= 0.0) { - return Double.POSITIVE_INFINITY; - } else { - return Double.NEGATIVE_INFINITY; - } - } - return sin_angle_ / cos_angle_; - } - - public double getRadians() { - return Math.atan2(sin_angle_, cos_angle_); - } - - public double getDegrees() { - return Math.toDegrees(getRadians()); - } - - /** - * We can rotate this Rotation2d by adding together the effects of it and - * another rotation. - * - * @param other - * The other rotation. See: - * https://en.wikipedia.org/wiki/Rotation_matrix - * @return This rotation rotated by other. - */ - public Rotation2d rotateBy(Rotation2d other) { - return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_, - cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true); - } - - /** - * The inverse of a Rotation2d "undoes" the effect of this rotation. - * - * @return The opposite of this rotation. - */ - public Rotation2d inverse() { - return new Rotation2d(cos_angle_, -sin_angle_, false); - } - - @Override - public String toString() { - final DecimalFormat fmt = new DecimalFormat("#0.000"); - return "(" + fmt.format(getDegrees()) + " deg)"; - } -} diff --git a/src/com/nutrons/libKudos254/vision/AdbBridge.java b/src/com/nutrons/libKudos254/vision/AdbBridge.java deleted file mode 100644 index a5e07ef..0000000 --- a/src/com/nutrons/libKudos254/vision/AdbBridge.java +++ /dev/null @@ -1,80 +0,0 @@ -package com.nutrons.libKudos254.vision; - -import java.io.IOException; -import java.nio.file.Path; -import java.nio.file.Paths; - -/** - * AdbBridge interfaces to an Android Debug Bridge (adb) binary, which is needed - * to communicate to Android devices over USB. - * - * adb binary provided by https://github.com/Spectrum3847/RIOdroid - */ -public class AdbBridge { - Path bin_location_; - public final static Path DEFAULT_LOCATION = Paths.get("/usr/bin/adb"); - - public AdbBridge() { - Path adb_location; - String env_val = System.getenv("FRC_ADB_LOCATION"); - if (env_val == null || "".equals(env_val)) { - adb_location = DEFAULT_LOCATION; - } else { - adb_location = Paths.get(env_val); - } - bin_location_ = adb_location; - } - - public AdbBridge(Path location) { - bin_location_ = location; - } - - private boolean runCommand(String args) { - Runtime r = Runtime.getRuntime(); - String cmd = bin_location_.toString() + " " + args; - - try { - Process p = r.exec(cmd); - p.waitFor(); - } catch (IOException e) { - System.err.println("AdbBridge: Could not run command " + cmd); - e.printStackTrace(); - return false; - } catch (InterruptedException e) { - System.err.println("AdbBridge: Could not run command " + cmd); - e.printStackTrace(); - return false; - } - return true; - } - - public void start() { - System.out.println("Starting adb"); - runCommand("start"); - } - - public void stop() { - System.out.println("Stopping adb"); - runCommand("kill-server"); - } - - public void restartAdb() { - System.out.println("Restarting adb"); - stop(); - start(); - } - - public void portForward(int local_port, int remote_port) { - runCommand("forward tcp:" + local_port + " tcp:" + remote_port); - } - - public void reversePortForward(int remote_port, int local_port) { - runCommand("reverse tcp:" + remote_port + " tcp:" + local_port); - } - - public void restartApp() { - System.out.println("Restarting app"); - runCommand("shell am force-stop com.team254.cheezdroid \\; " - + "am start com.team254.cheezdroid/com.team254.cheezdroid.VisionTrackerActivity"); - } -} diff --git a/src/com/nutrons/libKudos254/vision/CrashTracker.java b/src/com/nutrons/libKudos254/vision/CrashTracker.java deleted file mode 100644 index 6ddf767..0000000 --- a/src/com/nutrons/libKudos254/vision/CrashTracker.java +++ /dev/null @@ -1,66 +0,0 @@ -package com.nutrons.libKudos254.vision; - -import java.io.*; -import java.util.Date; -import java.util.UUID; - -/** - * Tracks start-up and caught crash events, logging them to a file which dosn't - * roll over - */ -public class CrashTracker { - - private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID(); - - public static void logRobotStartup() { - logMarker("robot startup"); - } - - public static void logRobotConstruction() { - logMarker("robot startup"); - } - - public static void logRobotInit() { - logMarker("robot init"); - } - - public static void logTeleopInit() { - logMarker("teleop init"); - } - - public static void logAutoInit() { - logMarker("auto init"); - } - - public static void logDisabledInit() { - logMarker("disabled init"); - } - - public static void logThrowableCrash(Throwable throwable) { - logMarker("Exception", throwable); - } - - private static void logMarker(String mark) { - logMarker(mark, null); - } - - private static void logMarker(String mark, Throwable nullableException) { - - try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) { - writer.print(RUN_INSTANCE_UUID.toString()); - writer.print(", "); - writer.print(mark); - writer.print(", "); - writer.print(new Date().toString()); - - if (nullableException != null) { - writer.print(", "); - nullableException.printStackTrace(writer); - } - - writer.println(); - } catch (IOException e) { - e.printStackTrace(); - } - } -} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java b/src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java deleted file mode 100644 index ce4bdf4..0000000 --- a/src/com/nutrons/libKudos254/vision/CrashTrackingRunnable.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.nutrons.libKudos254.vision; - -/** - * Runnable class with reports all uncaught throws to CrashTracker - */ -public abstract class CrashTrackingRunnable implements Runnable { - - @Override - public final void run() { - try { - runCrashTracked(); - } catch (Throwable t) { - CrashTracker.logThrowableCrash(t); - throw t; - } - } - - public abstract void runCrashTracked(); -} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/TargetInfo.java b/src/com/nutrons/libKudos254/vision/TargetInfo.java deleted file mode 100644 index 7a2903d..0000000 --- a/src/com/nutrons/libKudos254/vision/TargetInfo.java +++ /dev/null @@ -1,28 +0,0 @@ -package com.nutrons.libKudos254.vision; - -/** - * A container class for Targets detected by the vision system, containing the - * location in three-dimensional space. - */ -public class TargetInfo { - protected double x = 1.0; - protected double y; - protected double z; - - public TargetInfo(double y, double z) { - this.y = y; - this.z = z; - } - - public double getX() { - return x; - } - - public double getY() { - return y; - } - - public double getZ() { - return z; - } -} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/VisionServer.java b/src/com/nutrons/libKudos254/vision/VisionServer.java deleted file mode 100644 index 331c69e..0000000 --- a/src/com/nutrons/libKudos254/vision/VisionServer.java +++ /dev/null @@ -1,234 +0,0 @@ -package com.nutrons.libKudos254.vision; - -import com.nutrons.libKudos254.vision.messages.HeartbeatMessage; -import com.nutrons.libKudos254.vision.messages.OffWireMessage; -import com.nutrons.libKudos254.vision.messages.VisionMessage; -import edu.wpi.first.wpilibj.Timer; - -import java.io.IOException; -import java.io.InputStream; -import java.io.OutputStream; -import java.net.*; -import java.util.ArrayList; -import java.util.Collections; - -/** - * This controls all vision actions, including vision updates, capture, and - * interfacing with the Android phone with Android Debug Bridge. It also stores - * all VisionUpdates (from the Android phone) and contains methods to add - * to/prune the VisionUpdate list. Much like the subsystems, outside methods get - * the VisionServer instance (there is only one VisionServer) instead of - * creating new VisionServer instances. - * - * @see - */ - -public class VisionServer extends CrashTrackingRunnable { - - public static int kAndroidAppTcpPort = 8254; //TODO: MIGHT NEED TO CHANGE THIS - - private static VisionServer s_instance = null; - private ServerSocket m_server_socket; - private boolean m_running = true; - private int m_port; - private ArrayList receivers = new ArrayList<>(); - AdbBridge adb = new AdbBridge(); - double lastMessageReceivedTime = 0; - private boolean m_use_java_time = false; - - private ArrayList serverThreads = new ArrayList<>(); - private volatile boolean mWantsAppRestart = false; - - public static VisionServer getInstance() { - System.out.println("VisionServer instance created"); - if (s_instance == null) { - s_instance = new VisionServer(kAndroidAppTcpPort); - } - return s_instance; - } - - private boolean mIsConnect = false; - - public boolean isConnected() { - return mIsConnect; - } - - public void requestAppRestart() { - mWantsAppRestart = true; - } - - protected class ServerThread extends CrashTrackingRunnable { - private Socket m_socket; - - public ServerThread(Socket socket) { - m_socket = socket; - } - - public void send(VisionMessage message) { - String toSend = message.toJson() + "\n"; - if (m_socket != null && m_socket.isConnected()) { - try { - OutputStream os = m_socket.getOutputStream(); - os.write(toSend.getBytes()); - } catch (IOException e) { - System.out.println("Vision server IOException"); - System.err.println("VisionServer: Could not send data to socket"); - } - } - } - - public void handleMessage(VisionMessage message, double timestamp) { - if ("targets".equals(message.getType())) { - VisionUpdate update = VisionUpdate.generateFromJsonString(timestamp, message.getMessage()); - receivers.removeAll(Collections.singleton(null)); - if (update.isValid()) { - for (VisionUpdateReceiver receiver : receivers) { - receiver.gotUpdate(update); - } - } - } - if ("heartbeat".equals(message.getType())) { - send(HeartbeatMessage.getInstance()); - } - } - - public boolean isAlive() { - return m_socket != null && m_socket.isConnected() && !m_socket.isClosed(); - } - - @Override - public void runCrashTracked() { - if (m_socket == null) { - return; - } - try { - InputStream is = m_socket.getInputStream(); - byte[] buffer = new byte[2048]; - int read; - while (m_socket.isConnected() && (read = is.read(buffer)) != -1) { - double timestamp = getTimestamp(); - lastMessageReceivedTime = timestamp; - String messageRaw = new String(buffer, 0, read); - String[] messages = messageRaw.split("\n"); - for (String message : messages) { - OffWireMessage parsedMessage = new OffWireMessage(message); - if (parsedMessage.isValid()) { - handleMessage(parsedMessage, timestamp); - } - } - } - System.out.println("Socket disconnected"); - } catch (IOException e) { - System.err.println("Could not talk to socket"); - } - if (m_socket != null) { - try { - m_socket.close(); - } catch (IOException e) { - e.printStackTrace(); - } - } - } - } - - /** - * Instantializes the VisionServer and connects to ADB via the specified - * port. - * - * @param - */ - private VisionServer(int port) { - try { - adb = new AdbBridge(); - m_port = port; - m_server_socket = new ServerSocket(port); - adb.start(); - adb.reversePortForward(port, port); - try { - String useJavaTime = System.getenv("USE_JAVA_TIME"); - m_use_java_time = "true".equals(useJavaTime); - } catch (NullPointerException e) { - m_use_java_time = false; - } - } catch (IOException e) { - e.printStackTrace(); - } - new Thread(this).start(); - new Thread(new AppMaintainanceThread()).start(); - } - - public void restartAdb() { - adb.restartAdb(); - adb.reversePortForward(m_port, m_port); - } - - /** - * If a VisionUpdate object (i.e. a target) is not in the list, add it. - * - * @see VisionUpdate - */ - public void addVisionUpdateReceiver(VisionUpdateReceiver receiver) { - System.out.println("added vision update receiver"); - if (!receivers.contains(receiver)) { - receivers.add(receiver); - } - } - - public void removeVisionUpdateReceiver(VisionUpdateReceiver receiver) { - if (receivers.contains(receiver)) { - receivers.remove(receiver); - } - } - - @Override - public void runCrashTracked() { - while (m_running) { - try { - Socket p = m_server_socket.accept(); - ServerThread s = new ServerThread(p); - new Thread(s).start(); - serverThreads.add(s); - } catch (IOException e) { - System.err.println("Issue accepting socket connection!"); - } finally { - try { - Thread.sleep(100); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } - } - - private class AppMaintainanceThread extends CrashTrackingRunnable { - @Override - public void runCrashTracked() { - while (true) { - if (getTimestamp() - lastMessageReceivedTime > .1) { - // camera disconnected - adb.reversePortForward(m_port, m_port); - mIsConnect = false; - } else { - mIsConnect = true; - } - if (mWantsAppRestart) { - adb.restartApp(); - mWantsAppRestart = false; - } - try { - Thread.sleep(200); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } - } - - private double getTimestamp() { - if (m_use_java_time) { - return System.currentTimeMillis(); - } else { - return Timer.getFPGATimestamp(); - } - } -} diff --git a/src/com/nutrons/libKudos254/vision/VisionServerTest.java b/src/com/nutrons/libKudos254/vision/VisionServerTest.java deleted file mode 100644 index 7260117..0000000 --- a/src/com/nutrons/libKudos254/vision/VisionServerTest.java +++ /dev/null @@ -1,29 +0,0 @@ -package com.nutrons.libKudos254.vision; - -/** - * Tests the vision system by getting targets - */ -public class VisionServerTest { - public static class TestReceiver implements VisionUpdateReceiver { - @Override - public void gotUpdate(VisionUpdate update) { - System.out.println("num targets: " + update.getTargets().size()); - for (int i = 0; i < update.getTargets().size(); i++) { - TargetInfo target = update.getTargets().get(i); - System.out.println("Target: " + target.getY() + ", " + target.getZ()); - } - } - } - - public static void main(String[] args) { - VisionServer visionServer = VisionServer.getInstance(); - visionServer.addVisionUpdateReceiver(new TestReceiver()); - while (true) { - try { - Thread.sleep(100); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } -} diff --git a/src/com/nutrons/libKudos254/vision/VisionUpdate.java b/src/com/nutrons/libKudos254/vision/VisionUpdate.java deleted file mode 100644 index a1aa122..0000000 --- a/src/com/nutrons/libKudos254/vision/VisionUpdate.java +++ /dev/null @@ -1,102 +0,0 @@ -package com.nutrons.libKudos254.vision; - -import org.json.simple.JSONArray; -import org.json.simple.JSONObject; -import org.json.simple.parser.JSONParser; -import org.json.simple.parser.ParseException; - -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -/** - * VisionUpdate contains the various attributes outputted by the vision system, - * namely a list of targets and the timestamp at which it was captured. - */ -public class VisionUpdate { - protected boolean valid = false; - protected long capturedAgoMs; - protected List targets; - protected double capturedAtTimestamp = 0; - - private static long getOptLong(Object n, long defaultValue) { - if (n == null) { - return defaultValue; - } - return (long) n; - } - - private static JSONParser parser = new JSONParser(); - - private static Optional parseDouble(JSONObject j, String key) throws ClassCastException { - Object d = j.get(key); - if (d == null) { - return Optional.empty(); - } else { - return Optional.of((double) d); - } - } - - /** - * Generates a VisionUpdate object given a JSON blob and a timestamp. - * - * @param Capture - * timestamp - * @param JSON - * blob with update string, example: { "capturedAgoMs" : 100, - * "targets": [{"y": 5.4, "z": 5.5}] } - * @return VisionUpdate object - */ - // - public static VisionUpdate generateFromJsonString(double current_time, String updateString) { - VisionUpdate update = new VisionUpdate(); - try { - JSONObject j = (JSONObject) parser.parse(updateString); - long capturedAgoMs = getOptLong(j.get("capturedAgoMs"), 0); - if (capturedAgoMs == 0) { - update.valid = false; - return update; - } - update.capturedAgoMs = capturedAgoMs; - update.capturedAtTimestamp = current_time - capturedAgoMs / 1000.0; - JSONArray targets = (JSONArray) j.get("targets"); - ArrayList targetInfos = new ArrayList<>(targets.size()); - for (Object targetObj : targets) { - JSONObject target = (JSONObject) targetObj; - Optional y = parseDouble(target, "y"); - Optional z = parseDouble(target, "z"); - if (!(y.isPresent() && z.isPresent())) { - update.valid = false; - return update; - } - targetInfos.add(new TargetInfo(y.get(), z.get())); - } - update.targets = targetInfos; - update.valid = true; - } catch (ParseException e) { - System.err.println("Parse error: " + e); - System.err.println(updateString); - } catch (ClassCastException e) { - System.err.println("Data type error: " + e); - System.err.println(updateString); - } - return update; - } - - public List getTargets() { - return targets; - } - - public boolean isValid() { - return valid; - } - - public long getCapturedAgoMs() { - return capturedAgoMs; - } - - public double getCapturedAtTimestamp() { - return capturedAtTimestamp; - } - -} diff --git a/src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java b/src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java deleted file mode 100644 index 87a0c15..0000000 --- a/src/com/nutrons/libKudos254/vision/VisionUpdateReceiver.java +++ /dev/null @@ -1,11 +0,0 @@ -package com.nutrons.libKudos254.vision; - -/** - * A basic interface for classes that get VisionUpdates. Classes that implement - * this interface specify what to do when VisionUpdates are received. - * - * @see VisionUpdate.java - */ -public interface VisionUpdateReceiver { - void gotUpdate(VisionUpdate update); -} \ No newline at end of file diff --git a/src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java b/src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java deleted file mode 100644 index aee19ec..0000000 --- a/src/com/nutrons/libKudos254/vision/messages/HeartbeatMessage.java +++ /dev/null @@ -1,27 +0,0 @@ -package com.nutrons.libKudos254.vision.messages; - -/** - * A message that acts as a "heartbeat"- ensures that the vision system is - * working. The message simply contains the instance of the VisionServer object. - */ -public class HeartbeatMessage extends VisionMessage { - - static HeartbeatMessage sInst = null; - - public static HeartbeatMessage getInstance() { - if (sInst == null) { - sInst = new HeartbeatMessage(); - } - return sInst; - } - - @Override - public String getType() { - return "heartbeat"; - } - - @Override - public String getMessage() { - return "{}"; - } -} diff --git a/src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java b/src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java deleted file mode 100644 index 5bb42d4..0000000 --- a/src/com/nutrons/libKudos254/vision/messages/OffWireMessage.java +++ /dev/null @@ -1,41 +0,0 @@ -package com.nutrons.libKudos254.vision.messages; - -import org.json.simple.JSONObject; -import org.json.simple.parser.JSONParser; -import org.json.simple.parser.ParseException; - -/** - * Used to convert Strings into OffWireMessage objects, which can be interpreted - * as generic VisionMessages. - */ -public class OffWireMessage extends VisionMessage { - - private boolean mValid = false; - private String mType = "unknown"; - private String mMessage = "{}"; - - public OffWireMessage(String message) { - JSONParser parser = new JSONParser(); - try { - JSONObject j = (JSONObject) parser.parse(message); - mType = (String) j.get("type"); - mMessage = (String) j.get("message"); - mValid = true; - } catch (ParseException e) { - } - } - - public boolean isValid() { - return mValid; - } - - @Override - public String getType() { - return mType; - } - - @Override - public String getMessage() { - return mMessage; - } -} diff --git a/src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java b/src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java deleted file mode 100644 index 9ddefe7..0000000 --- a/src/com/nutrons/libKudos254/vision/messages/SetCameraModeMessage.java +++ /dev/null @@ -1,35 +0,0 @@ -package com.nutrons.libKudos254.vision.messages; - -/** - * A Message that contains and can set the state of the camera and intake - * systems. - */ -public class SetCameraModeMessage extends VisionMessage { - - private static final String K_VISION_MODE = "vision"; - private static final String K_INTAKE_MODE = "intake"; - - private String mMessage = K_VISION_MODE; - - private SetCameraModeMessage(String message) { - mMessage = message; - } - - public static SetCameraModeMessage getVisionModeMessage() { - return new SetCameraModeMessage(K_VISION_MODE); - } - - public static SetCameraModeMessage getIntakeModeMessage() { - return new SetCameraModeMessage(K_INTAKE_MODE); - } - - @Override - public String getType() { - return "camera_mode"; - } - - @Override - public String getMessage() { - return mMessage; - } -} diff --git a/src/com/nutrons/libKudos254/vision/messages/VisionMessage.java b/src/com/nutrons/libKudos254/vision/messages/VisionMessage.java deleted file mode 100644 index 141b460..0000000 --- a/src/com/nutrons/libKudos254/vision/messages/VisionMessage.java +++ /dev/null @@ -1,21 +0,0 @@ -package com.nutrons.libKudos254.vision.messages; - -import org.json.simple.JSONObject; - -/** - * An abstract class used for messages about the vision subsystem. - */ -public abstract class VisionMessage { - - public abstract String getType(); - - public abstract String getMessage(); - - public String toJson() { - JSONObject j = new JSONObject(); - j.put("type", getType()); - j.put("message", getMessage()); - return j.toString(); - } - -} From c53c33e9eb4b73c0538899f1a4c31b8c986b6f64 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Mon, 27 Feb 2017 19:57:30 -0500 Subject: [PATCH 28/45] fromSwitch idempotency fixed, changed method signature to enable different uses --- .../nutrons/framework/commands/Command.java | 34 ++++++++++++------- .../nutrons/framework/test/TestCommand.java | 9 +++++ .../framework/test/TestSwitchCommand.java | 10 +++--- 3 files changed, 36 insertions(+), 17 deletions(-) diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 7cd1566..cae42a0 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -70,21 +70,30 @@ public static Command parallel(Command... commands) { return new Command(new ParallelCommand(commands)); } + public static Command fromSwitch(Publisher commandStream, boolean subcommandsSelfTerminate) { + return fromSwitch(commandStream, subcommandsSelfTerminate, true); + } + /** - * Creates a command that runs sequentially to another. + * Creates a command that runs commands in a stream. * - * @param commandStream A flowable of commands + * @param commandStream A flowable of commands + * @param subcommandsSelfTerminate if true, commands in the stream will self terminate; + * @param subcommandsForceTerminate if true, commands in the stream will terminate the previous command. * @retuns Second command after first is executed. */ - public static Command fromSwitch(Publisher commandStream) { - return new Command(x -> Flowable.defer(() -> - Flowable.fromPublisher(commandStream) - .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(x))) - .subscribeOn(Schedulers.io())) - .scan((a, b) -> { - a.run(); - return b; - }))); + public static Command fromSwitch(Publisher commandStream, + boolean subcommandsSelfTerminate, + boolean subcommandsForceTerminate) { + return new Command(x -> Flowable.fromPublisher(commandStream) + .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(subcommandsSelfTerminate))) + .subscribeOn(Schedulers.io())) + .scan((a, b) -> { + if (subcommandsForceTerminate) { + a.run(); + } + return b; + }).replay().autoConnect()); } public Command addFinalTerminator(Terminator terminator) { @@ -182,7 +191,8 @@ public Flowable execute(boolean selfTerminating) { Flowable terms = source.execute(selfTerminating) .subscribeOn(Schedulers.io()); if (selfTerminating) { - terms.toList().subscribe(x -> Observable.fromIterable(x).blockingSubscribe(Terminator::run)); + terms.toList().map(Observable::fromIterable).subscribeOn(Schedulers.io()) + .subscribe(x -> x.subscribe(Terminator::run)); } return terms; } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index abbc0c2..9ce69c1 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -141,4 +141,13 @@ public void killAfter() throws InterruptedException { Thread.sleep(4000); assertTrue(record[0] == 1); } + + @Test + public void notSelfTerminating() throws InterruptedException { + Command doesntTermrinate = Command.just(x -> + Flowable.just(() -> assertTrue(false)) + .mergeWith(Flowable.never())); + doesntTermrinate.execute(true); + Thread.sleep(2000); + } } diff --git a/test/com/nutrons/framework/test/TestSwitchCommand.java b/test/com/nutrons/framework/test/TestSwitchCommand.java index 1df5d45..22658af 100644 --- a/test/com/nutrons/framework/test/TestSwitchCommand.java +++ b/test/com/nutrons/framework/test/TestSwitchCommand.java @@ -18,7 +18,7 @@ public void testSwitchStart() throws InterruptedException { List list = new ArrayList<>(); Flowable commandStream = Flowable.interval(100, TimeUnit.MILLISECONDS) .map(x -> putNumber(list, x)).take(5); - Command.fromSwitch(commandStream).execute(true); + Command.fromSwitch(commandStream, true).execute(true); Thread.sleep(300); assertFalse(list.contains(4)); Thread.sleep(500); @@ -46,7 +46,7 @@ public void testSwitchStartAndStop() throws InterruptedException { } })); }); - Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) + Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5), true) .execute(true).blockingSubscribe(); Thread.sleep(2000); assertTrue(record[0] == 0); @@ -62,7 +62,7 @@ public void testSwitchTerminateRealtime() throws InterruptedException { record[0] += 1; } }))); - Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5)) + Command.fromSwitch(Flowable.interval(1, TimeUnit.SECONDS).map(x -> inc).take(5), true) .execute(true); assertTrue(System.currentTimeMillis() - start < 1000); Thread.sleep(2000); @@ -76,7 +76,7 @@ public void testSwitchNotTerminating() throws InterruptedException { doesntFinish.execute(false); Thread.sleep(1000); Command justOne = Command.fromSwitch(Flowable.never() - .mergeWith(Flowable.just(doesntFinish))); + .mergeWith(Flowable.just(doesntFinish)), false); justOne.execute(false); Thread.sleep(1000); justOne.execute(true); @@ -88,7 +88,7 @@ public void testSwitchTerminatesOnNext() { Command doesntFinish = Command.just(x -> Flowable.just(() -> { throw new RuntimeException(); })); - Command two = Command.fromSwitch(Flowable.just(doesntFinish, doesntFinish)); + Command two = Command.fromSwitch(Flowable.just(doesntFinish, doesntFinish), false); two.execute(false).blockingSubscribe(); } } From f624c1624956dd825f419ebd78e627be5cccc495 Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Thu, 2 Mar 2017 09:13:45 -0500 Subject: [PATCH 29/45] added radio box --- .../nutrons/framework/inputs/RadioBox.java | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 src/com/nutrons/framework/inputs/RadioBox.java diff --git a/src/com/nutrons/framework/inputs/RadioBox.java b/src/com/nutrons/framework/inputs/RadioBox.java new file mode 100644 index 0000000..fdca643 --- /dev/null +++ b/src/com/nutrons/framework/inputs/RadioBox.java @@ -0,0 +1,43 @@ +package com.nutrons.framework.inputs; + +import static com.nutrons.framework.util.FlowOperators.toFlow; + +import com.nutrons.framework.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import io.reactivex.Flowable; +import io.reactivex.Observable; +import java.util.HashMap; +import java.util.Map; + +public class RadioBox implements Subsystem { + private final SendableChooser chooser; + private final Map options; + private final String name; + private Map.Entry defaultOption; + + public RadioBox(String name, Map options, String defaultOption) { + this.name = name; + this.chooser = new SendableChooser<>(); + this.options = new HashMap<>(); + Observable> optionStream = Observable.fromIterable(options.entrySet()); + optionStream.filter(x -> x.getKey().equals(defaultOption)) + .blockingSubscribe(x -> this.defaultOption = x); + optionStream.filter(x -> !x.getKey().equals(defaultOption)) + .blockingSubscribe(x -> this.options.put(x.getKey(), x.getValue())); + } + + @Override + public void registerSubscriptions() { + if (defaultOption != null) { + chooser.addDefault(defaultOption.getKey(), defaultOption.getValue()); + } + Observable.fromIterable(options.entrySet()).blockingSubscribe(x -> + chooser.addObject(x.getKey(), x.getValue())); + SmartDashboard.putData(name, chooser); + } + + public Flowable selected() { + return toFlow(chooser::getSelected).distinctUntilChanged(); + } +} From 052a83f3b8821e689a9d6cc3fac0ee7c5d6b6cb0 Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Thu, 2 Mar 2017 09:32:51 -0500 Subject: [PATCH 30/45] added defer command constructor --- src/com/nutrons/framework/commands/Command.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 25d31dd..e032467 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -9,6 +9,7 @@ import io.reactivex.disposables.Disposable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.schedulers.Schedulers; + import java.util.concurrent.TimeUnit; import java.util.function.Supplier; @@ -202,4 +203,11 @@ public Flowable execute(boolean selfTerminating) { } return terms; } + + public static Command defer(Supplier supplier) { + return Command.just(x -> { + Command actual = supplier.get(); + return actual.execute(x); + }); + } } From 4e24d483ed47d82e690925e7e54734fcbd580711 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Thu, 2 Mar 2017 21:24:30 -0500 Subject: [PATCH 31/45] added test to make sure Connectable Flowables work the way we expect --- .../nutrons/framework/test/TestConnectables.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/test/com/nutrons/framework/test/TestConnectables.java b/test/com/nutrons/framework/test/TestConnectables.java index 0de262d..e83ba6e 100644 --- a/test/com/nutrons/framework/test/TestConnectables.java +++ b/test/com/nutrons/framework/test/TestConnectables.java @@ -27,4 +27,18 @@ public void testConnectableDiscards() throws InterruptedException { flow.take(1).subscribeOn(Schedulers.io()).subscribe(x -> assertTrue(x != 0)); Thread.sleep(1000); } + + @Test + public void testConnectableWaitsToRetrieve() throws InterruptedException { + Runnable avoid = () -> assertTrue(false); + Runnable wanted = () -> {}; + ConnectableFlowable source = Flowable.interval(1000, TimeUnit.MILLISECONDS, Schedulers.io()).map(x -> avoid) + .take(5).concatWith(Flowable.timer(3, TimeUnit.SECONDS).map(x -> wanted)).publish(); + source.connect(); + ConnectableFlowable listener = source.take(1).publish(); + listener.subscribe(Runnable::run); + Thread.sleep(6000); + listener.connect(); + Thread.sleep(1000); + } } From 496e424c495a76b2c4ccf3121c04ad2e187fdb9a Mon Sep 17 00:00:00 2001 From: Camilo Gonzalez Date: Fri, 3 Mar 2017 17:17:27 -0500 Subject: [PATCH 32/45] 40kpa auto now working, just short --- src/com/nutrons/framework/commands/Command.java | 6 +++--- src/com/nutrons/framework/commands/ParallelCommand.java | 2 +- src/com/nutrons/framework/commands/SerialCommand.java | 2 +- src/com/nutrons/framework/util/FlowOperators.java | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 205de46..66fc388 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -85,7 +85,7 @@ public static Command fromSwitch(Publisher commandStr public static Command fromSwitch(Publisher commandStream, boolean subcommandsSelfTerminate, boolean subcommandsForceTerminate) { - return new Command(x -> Flowable.fromPublisher(commandStream) + return new Command(x -> Flowable.fromPublisher(commandStream).share() .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(subcommandsSelfTerminate))) .subscribeOn(Schedulers.io())) .scan((a, b) -> { @@ -93,7 +93,7 @@ public static Command fromSwitch(Publisher commandStr a.run(); } return b; - }).replay().autoConnect()); + }).share()); } public static Command defer(Supplier supplier) { @@ -178,7 +178,7 @@ public Command then(Command next) { * Copies this command into one which will delay execution for a period of time. */ public Command delayStart(long delay, TimeUnit unit) { - return this.startable(Flowable.timer(delay, unit)); + return this.startable(Flowable.timer(delay, unit).onBackpressureBuffer()); } /** diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index 53b05a6..b7d2b2e 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -13,7 +13,7 @@ public class ParallelCommand implements CommandWorkUnit { @Override public Flowable execute(boolean selfTerminating) { - Flowable terminators = this.commands + Flowable terminators = this.commands .flatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); return Flowable.just(FlattenedTerminator.from(terminators)) diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index ac968b5..2ff8944 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -13,7 +13,7 @@ public class SerialCommand implements CommandWorkUnit { @Override public Flowable execute(boolean selfTerminating) { - Flowable terminators = this.commands + Flowable terminators = this.commands .concatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); return Flowable.just(FlattenedTerminator.from(terminators)) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index db3b2de..f2b82f7 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -27,7 +27,7 @@ public static Flowable toFlow(Supplier supplier, long ignored, TimeUnit unit) { return Flowable.interval(ignored, unit).subscribeOn(Schedulers.io()) .map(x -> supplier.get()).onBackpressureDrop().observeOn(Schedulers.computation()) - .onBackpressureDrop(); + .onBackpressureDrop().share(); } /** From de2b6d812f98deb8a82d0d113819659c760f377c Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Fri, 3 Mar 2017 23:42:08 -0500 Subject: [PATCH 33/45] added pdLoop operator, fixed small possible bug in Parrallel + Serial commands --- .../framework/commands/ParallelCommand.java | 2 +- .../framework/commands/SerialCommand.java | 2 +- .../nutrons/framework/util/FlowOperators.java | 27 +++++++++++++------ 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index 53b05a6..b7d2b2e 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -13,7 +13,7 @@ public class ParallelCommand implements CommandWorkUnit { @Override public Flowable execute(boolean selfTerminating) { - Flowable terminators = this.commands + Flowable terminators = this.commands .flatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); return Flowable.just(FlattenedTerminator.from(terminators)) diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index ac968b5..2ff8944 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -13,7 +13,7 @@ public class SerialCommand implements CommandWorkUnit { @Override public Flowable execute(boolean selfTerminating) { - Flowable terminators = this.commands + Flowable terminators = this.commands .concatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) .subscribeOn(Schedulers.io()).publish().autoConnect(); return Flowable.just(FlattenedTerminator.from(terminators)) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index db3b2de..eeb8aae 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -14,17 +14,16 @@ import java.util.function.Supplier; - 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(); @@ -58,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); } @@ -67,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; } @@ -79,9 +78,9 @@ 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 error -> { Flowable errorP = error.map(x -> x * proportional); Flowable errorI = error.buffer(integralBuffer, 1) @@ -96,6 +95,17 @@ public static FlowableTransformer pidLoop(double proportional, }; } + public static FlowableTransformer pdLoop(double proportional, + double derivative) { + 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(); + }; + } + public static Function limitWithin(double minimum, double maximum) { return x -> max(min(x, maximum), minimum); } @@ -107,6 +117,7 @@ public static T printId(T flowables) { /** * Combines disposable given to this method in order to return them all at once. + * * @param disposables the disposables you desire to combine * @return combined disposables. */ From a9a12d1084977a0c218363824abe81b99ea20931 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Sat, 4 Mar 2017 15:33:43 -0500 Subject: [PATCH 34/45] updated command subscription shenanigans --- src/com/nutrons/framework/commands/Command.java | 15 +++++++-------- .../framework/commands/FlattenedTerminator.java | 2 +- .../framework/commands/ParallelCommand.java | 5 ++--- .../nutrons/framework/commands/SerialCommand.java | 5 ++--- 4 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 66fc388..2f13a39 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -85,7 +85,7 @@ public static Command fromSwitch(Publisher commandStr public static Command fromSwitch(Publisher commandStream, boolean subcommandsSelfTerminate, boolean subcommandsForceTerminate) { - return new Command(x -> Flowable.fromPublisher(commandStream).share() + return new Command(x -> Flowable.fromPublisher(commandStream) .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(subcommandsSelfTerminate))) .subscribeOn(Schedulers.io())) .scan((a, b) -> { @@ -93,7 +93,7 @@ public static Command fromSwitch(Publisher commandStr a.run(); } return b; - }).share()); + })); } public static Command defer(Supplier supplier) { @@ -111,7 +111,7 @@ public static Command defer(Supplier supplier) { */ public Command addFinalTerminator(Terminator terminator) { return Command.just(x -> this.source.execute(x) - .flatMap(y -> Flowable.just(y, terminator)).subscribeOn(Schedulers.io())); + .flatMap(y -> Flowable.just(y, terminator))); } /** @@ -119,7 +119,7 @@ public Command addFinalTerminator(Terminator terminator) { */ public Command startable(Publisher starter) { return new Command(new SerialCommand( - Command.just(x -> Flowable.defer(() -> Flowable.never().takeUntil(starter))), + Command.just(x -> Flowable.never().takeUntil(starter)), this)); } @@ -128,8 +128,7 @@ public Command startable(Publisher starter) { * will delay the execution of all actions until startCondition returns true. */ public Command when(Supplier startCondition) { - Flowable ignition = Flowable - .defer(() -> emptyPulse.map(x -> startCondition.get()).filter(x -> x).onBackpressureDrop()); + Flowable ignition = emptyPulse.map(x -> startCondition.get()).filter(x -> x).onBackpressureDrop(); return this.startable(ignition); } @@ -151,8 +150,8 @@ public Command endsWhen(Publisher terminator, boolean terminatesAtEnd) { return Command.just(x -> { Flowable sourceTerminator = this.execute(terminatesAtEnd); Terminator multi = FlattenedTerminator.from(sourceTerminator); - return Flowable.defer(() -> Flowable.never().takeUntil(terminator) - .mergeWith(Flowable.just(multi::run))); + return Flowable.never().takeUntil(terminator) + .mergeWith(Flowable.just(multi)); }); } diff --git a/src/com/nutrons/framework/commands/FlattenedTerminator.java b/src/com/nutrons/framework/commands/FlattenedTerminator.java index 769603c..3a727c8 100644 --- a/src/com/nutrons/framework/commands/FlattenedTerminator.java +++ b/src/com/nutrons/framework/commands/FlattenedTerminator.java @@ -16,7 +16,7 @@ private FlattenedTerminator(Flowable terminators) { this.lock = new AtomicBoolean(false); this.terminators = new ArrayList<>(); this.terminatorStream = terminators; - terminators.subscribeOn(Schedulers.io()).subscribe(x -> { + terminators.subscribe(x -> { if (!lock.get()) { synchronized (lock) { if (!lock.get()) { diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index b7d2b2e..9c877a9 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -14,9 +14,8 @@ public class ParallelCommand implements CommandWorkUnit { @Override public Flowable execute(boolean selfTerminating) { Flowable terminators = this.commands - .flatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) - .subscribeOn(Schedulers.io()).publish().autoConnect(); + .flatMap(x -> x.execute(selfTerminating)); return Flowable.just(FlattenedTerminator.from(terminators)) - .mergeWith(terminators.ignoreElements().toFlowable()); + .mergeWith(terminators.ignoreElements().toFlowable()).subscribeOn(Schedulers.io()); } } diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index 2ff8944..de07347 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -14,9 +14,8 @@ public class SerialCommand implements CommandWorkUnit { @Override public Flowable execute(boolean selfTerminating) { Flowable terminators = this.commands - .concatMap(x -> x.execute(selfTerminating).subscribeOn(Schedulers.io())) - .subscribeOn(Schedulers.io()).publish().autoConnect(); + .concatMap(x -> x.execute(selfTerminating)); return Flowable.just(FlattenedTerminator.from(terminators)) - .mergeWith(terminators.ignoreElements().toFlowable()); + .mergeWith(terminators.ignoreElements().toFlowable()).subscribeOn(Schedulers.io()); } } From fb9db0b26425e59932bc62a3a3669a21e3c2f1f4 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Tue, 7 Mar 2017 18:06:16 -0500 Subject: [PATCH 35/45] actually really actually fixed memory --- src/com/nutrons/framework/StreamManager.java | 4 ++-- src/com/nutrons/framework/commands/Command.java | 8 +++++--- .../framework/commands/FlattenedTerminator.java | 2 +- .../nutrons/framework/commands/ParallelCommand.java | 12 ++++++++---- .../nutrons/framework/commands/SerialCommand.java | 12 ++++++++---- src/com/nutrons/framework/util/FlowOperators.java | 12 +++++++----- test/com/nutrons/framework/test/TestCommand.java | 12 ++++++++++++ 7 files changed, 43 insertions(+), 19 deletions(-) diff --git a/src/com/nutrons/framework/StreamManager.java b/src/com/nutrons/framework/StreamManager.java index 891cccc..dfb7d55 100644 --- a/src/com/nutrons/framework/StreamManager.java +++ b/src/com/nutrons/framework/StreamManager.java @@ -62,7 +62,7 @@ public final void startCompetition(Supplier autoSupplier, System.out.println("all subsystems registered"); Command auto = autoSupplier.get(); if (auto != null) { - combineLatest(this.enabled, this.mode, (x, y) -> x && y.compareTo(AUTO) == 0) + combineLatest(this.enabled, this.mode, (x, y) -> x && y.compareTo(AUTO) == 0).onBackpressureDrop() .subscribeOn(Schedulers.io()) .filter(x -> x) .map((a) -> { @@ -73,7 +73,7 @@ public final void startCompetition(Supplier autoSupplier, } Command tele = teleopSupplier.get(); if (tele != null) { - combineLatest(this.enabled, this.mode, (x, y) -> x && y.compareTo(TELE) == 0) + combineLatest(this.enabled, this.mode, (x, y) -> x && y.compareTo(TELE) == 0).onBackpressureDrop() .subscribeOn(Schedulers.io()) .filter(x -> x) .map((a) -> { diff --git a/src/com/nutrons/framework/commands/Command.java b/src/com/nutrons/framework/commands/Command.java index 2f13a39..7ddbb0b 100644 --- a/src/com/nutrons/framework/commands/Command.java +++ b/src/com/nutrons/framework/commands/Command.java @@ -87,7 +87,7 @@ public static Command fromSwitch(Publisher commandStr boolean subcommandsForceTerminate) { return new Command(x -> Flowable.fromPublisher(commandStream) .concatMap(y -> Flowable.just(FlattenedTerminator.from(y.execute(subcommandsSelfTerminate))) - .subscribeOn(Schedulers.io())) + .subscribeOn(Schedulers.trampoline())) .scan((a, b) -> { if (subcommandsForceTerminate) { a.run(); @@ -206,10 +206,12 @@ public Command killAfter(long delay, TimeUnit unit) { @Override public Flowable execute(boolean selfTerminating) { Flowable terms = source.execute(selfTerminating) - .subscribeOn(Schedulers.io()); + .subscribeOn(Schedulers.trampoline()); if (selfTerminating) { - terms.toList().map(Observable::fromIterable).subscribeOn(Schedulers.io()) + terms.toList().map(Observable::fromIterable).subscribeOn(Schedulers.trampoline()) .subscribe(x -> x.subscribe(Terminator::run)); + } else { + terms.subscribe(); } return terms; } diff --git a/src/com/nutrons/framework/commands/FlattenedTerminator.java b/src/com/nutrons/framework/commands/FlattenedTerminator.java index 3a727c8..04aa03c 100644 --- a/src/com/nutrons/framework/commands/FlattenedTerminator.java +++ b/src/com/nutrons/framework/commands/FlattenedTerminator.java @@ -35,7 +35,7 @@ static FlattenedTerminator from(Flowable terminators) { public Flowable toSingle() { return Flowable.just(this).mergeWith(terminatorStream.ignoreElements().toFlowable()) - .subscribeOn(Schedulers.io()); + .subscribeOn(Schedulers.trampoline()); } @Override diff --git a/src/com/nutrons/framework/commands/ParallelCommand.java b/src/com/nutrons/framework/commands/ParallelCommand.java index 9c877a9..3b5b021 100644 --- a/src/com/nutrons/framework/commands/ParallelCommand.java +++ b/src/com/nutrons/framework/commands/ParallelCommand.java @@ -1,21 +1,25 @@ package com.nutrons.framework.commands; import io.reactivex.Flowable; -import io.reactivex.schedulers.Schedulers; public class ParallelCommand implements CommandWorkUnit { private final Flowable commands; ParallelCommand(CommandWorkUnit... commands) { - this.commands = Flowable.fromArray(commands).map(Command::just); + this.commands = Flowable.fromArray(commands).concatMap(x -> { + if (x instanceof ParallelCommand) { + return ((ParallelCommand) x).commands; + } + return Flowable.just(Command.just(x)); + }); } @Override public Flowable execute(boolean selfTerminating) { Flowable terminators = this.commands - .flatMap(x -> x.execute(selfTerminating)); + .flatMap(x -> x.execute(selfTerminating)).replay().autoConnect(); return Flowable.just(FlattenedTerminator.from(terminators)) - .mergeWith(terminators.ignoreElements().toFlowable()).subscribeOn(Schedulers.io()); + .mergeWith(terminators.ignoreElements().toFlowable()); } } diff --git a/src/com/nutrons/framework/commands/SerialCommand.java b/src/com/nutrons/framework/commands/SerialCommand.java index de07347..488d641 100644 --- a/src/com/nutrons/framework/commands/SerialCommand.java +++ b/src/com/nutrons/framework/commands/SerialCommand.java @@ -1,21 +1,25 @@ package com.nutrons.framework.commands; import io.reactivex.Flowable; -import io.reactivex.schedulers.Schedulers; public class SerialCommand implements CommandWorkUnit { private final Flowable commands; SerialCommand(CommandWorkUnit... commands) { - this.commands = Flowable.fromArray(commands).map(Command::just); + this.commands = Flowable.fromArray(commands).concatMap(x -> { + if (x instanceof SerialCommand) { + return ((SerialCommand) x).commands; + } + return Flowable.just(Command.just(x)); + }); } @Override public Flowable execute(boolean selfTerminating) { Flowable terminators = this.commands - .concatMap(x -> x.execute(selfTerminating)); + .concatMap(x -> x.execute(selfTerminating)).replay().autoConnect(); return Flowable.just(FlattenedTerminator.from(terminators)) - .mergeWith(terminators.ignoreElements().toFlowable()).subscribeOn(Schedulers.io()); + .mergeWith(terminators.ignoreElements().toFlowable()); } } diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 250b9d3..b305e43 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -8,6 +8,7 @@ import io.reactivex.FlowableTransformer; import io.reactivex.disposables.CompositeDisposable; import io.reactivex.disposables.Disposable; +import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.functions.Function; import io.reactivex.schedulers.Schedulers; import java.util.concurrent.TimeUnit; @@ -24,9 +25,10 @@ public class FlowOperators { */ public static Flowable toFlow(Supplier supplier, long ignored, TimeUnit unit) { - return Flowable.interval(ignored, unit).subscribeOn(Schedulers.io()) - .map(x -> supplier.get()).onBackpressureDrop().observeOn(Schedulers.computation()) - .onBackpressureDrop().share(); + ConnectableFlowable toRet = Flowable.interval(ignored, unit, Schedulers.io()) + .map(x -> supplier.get()).onBackpressureDrop().observeOn(Schedulers.computation()).publish(); + toRet.connect(); + return toRet; } /** @@ -90,7 +92,7 @@ public static FlowableTransformer pidLoop(double proportional, .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); + (p, i, d) -> p + i + d).publish().autoConnect(); return output; }; } @@ -102,7 +104,7 @@ public static FlowableTransformer pdLoop(double 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(); + return Flowable.combineLatest(errorP, errorD, (p, d) -> p + d).publish().autoConnect(); }; } diff --git a/test/com/nutrons/framework/test/TestCommand.java b/test/com/nutrons/framework/test/TestCommand.java index f211321..c55d8bd 100644 --- a/test/com/nutrons/framework/test/TestCommand.java +++ b/test/com/nutrons/framework/test/TestCommand.java @@ -31,6 +31,18 @@ public void setupCommands() { }).killAfter(1000, TimeUnit.MILLISECONDS); } + @Test + public void fromAction() throws InterruptedException { + int[] record = new int[1]; + Command.fromAction(() -> { + synchronized (record) { + record[0] = 1; + } + }).execute(true); + Thread.sleep(1000); + assertTrue(record[0] == 1); + } + @Test public void single() { final Integer[] arr = new Integer[1]; From fb01fef10bdace89368316b836fb8210b489ea1b Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Wed, 15 Mar 2017 17:30:00 -0400 Subject: [PATCH 36/45] added virtual controller support to events --- .../nutrons/framework/controllers/Events.java | 11 ++ .../framework/controllers/LoopModeEvent.java | 4 + .../controllers/LoopPropertiesEvent.java | 5 + .../controllers/ResetPositionEvent.java | 4 + .../controllers/RunAtPowerEvent.java | 5 + .../controllers/SetReversedSensorEvent.java | 5 + .../framework/controllers/SetpointEvent.java | 5 + .../controllers/VirtualSpeedController.java | 103 +++++++++++++++++- 8 files changed, 139 insertions(+), 3 deletions(-) diff --git a/src/com/nutrons/framework/controllers/Events.java b/src/com/nutrons/framework/controllers/Events.java index 63612b0..65b5464 100644 --- a/src/com/nutrons/framework/controllers/Events.java +++ b/src/com/nutrons/framework/controllers/Events.java @@ -26,6 +26,11 @@ public void actOn(Talon talon) { pid.actOn(talon); setpointEvent.actOn(talon); } + @Override + public void actOn(VirtualSpeedController controller) { + pid.actOn(controller); + setpointEvent.actOn(controller); + } }; } @@ -46,6 +51,12 @@ public void actOn(Talon talon) { e.actOn(talon); } } + @Override + public void actOn(VirtualSpeedController controller) { + for (ControllerEvent e : events) { + e.actOn(controller); + } + } }; } diff --git a/src/com/nutrons/framework/controllers/LoopModeEvent.java b/src/com/nutrons/framework/controllers/LoopModeEvent.java index 82dc206..ce8cefb 100644 --- a/src/com/nutrons/framework/controllers/LoopModeEvent.java +++ b/src/com/nutrons/framework/controllers/LoopModeEvent.java @@ -16,4 +16,8 @@ public LoopModeEvent(ControlMode mode) { public void actOn(Talon talon) { talon.changeControlMode(mode); } + + public void actOn(VirtualSpeedController controller) { + controller.changeControlMode(mode); + } } diff --git a/src/com/nutrons/framework/controllers/LoopPropertiesEvent.java b/src/com/nutrons/framework/controllers/LoopPropertiesEvent.java index b7c3c8b..cddab84 100644 --- a/src/com/nutrons/framework/controllers/LoopPropertiesEvent.java +++ b/src/com/nutrons/framework/controllers/LoopPropertiesEvent.java @@ -27,4 +27,9 @@ public void actOn(Talon talon) { talon.setD(dval); talon.setF(fval); } + + @Override + public void actOn(VirtualSpeedController controller) { + controller.setLoopProperties(pval, ival, dval, fval); + } } diff --git a/src/com/nutrons/framework/controllers/ResetPositionEvent.java b/src/com/nutrons/framework/controllers/ResetPositionEvent.java index e426607..69f35b3 100644 --- a/src/com/nutrons/framework/controllers/ResetPositionEvent.java +++ b/src/com/nutrons/framework/controllers/ResetPositionEvent.java @@ -16,4 +16,8 @@ public ResetPositionEvent(double position) { public void actOn(Talon talon) { talon.resetPositionTo(position); } + + public void actOn(VirtualSpeedController controller) { + controller.resetPositionTo(position); + } } diff --git a/src/com/nutrons/framework/controllers/RunAtPowerEvent.java b/src/com/nutrons/framework/controllers/RunAtPowerEvent.java index c460cac..f4cb29e 100644 --- a/src/com/nutrons/framework/controllers/RunAtPowerEvent.java +++ b/src/com/nutrons/framework/controllers/RunAtPowerEvent.java @@ -26,4 +26,9 @@ public void actOn(Talon talon) { } talon.set(power); } + + public void actOn(VirtualSpeedController controller) { + controller.setControlMode(ControlMode.MANUAL); + controller.setRawOutput(power); + } } diff --git a/src/com/nutrons/framework/controllers/SetReversedSensorEvent.java b/src/com/nutrons/framework/controllers/SetReversedSensorEvent.java index c58e215..c1d0aad 100644 --- a/src/com/nutrons/framework/controllers/SetReversedSensorEvent.java +++ b/src/com/nutrons/framework/controllers/SetReversedSensorEvent.java @@ -12,4 +12,9 @@ public SetReversedSensorEvent(boolean flipped) { public void actOn(Talon talon) { talon.reverseSensor(flipped); } + + @Override + public void actOn(VirtualSpeedController controller) { + controller.reverseSensor(true); + } } diff --git a/src/com/nutrons/framework/controllers/SetpointEvent.java b/src/com/nutrons/framework/controllers/SetpointEvent.java index 469028c..63086e8 100644 --- a/src/com/nutrons/framework/controllers/SetpointEvent.java +++ b/src/com/nutrons/framework/controllers/SetpointEvent.java @@ -12,4 +12,9 @@ class SetpointEvent implements ControllerEvent { public void actOn(Talon talon) { talon.changeSetpoint(setpoint); } + + @Override + public void actOn(VirtualSpeedController controller) { + controller.changeSetpoint(setpoint); + } } diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index 730738e..5913467 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -1,9 +1,58 @@ package com.nutrons.framework.controllers; +import com.nutrons.framework.util.FlowOperators; import io.reactivex.Flowable; +import io.reactivex.flowables.ConnectableFlowable; +import io.reactivex.processors.PublishProcessor; public class VirtualSpeedController extends LoopSpeedController { + private final PublishProcessor outputDirection; + private final ConnectableFlowable outputDirectionReplay; + private final PublishProcessor pidProperties; + private final ConnectableFlowable pidPropertiesReplay; + private final ConnectableFlowable position; + private final ConnectableFlowable speed; + private final PublishProcessor setpoint; + private final ConnectableFlowable setpointReplay; + private final PublishProcessor rawOutput; + private final ConnectableFlowable rawOutputReplay; + private final PublishProcessor mode; + private final ConnectableFlowable modeReplay; + private final PublishProcessor sensorDirection; + private final ConnectableFlowable sensorDirectionRelay; + + public VirtualSpeedController(Flowable position, Flowable speed) { + this.outputDirection = PublishProcessor.create(); + this.outputDirectionReplay = this.outputDirection.distinctUntilChanged().replay(1); + this.pidProperties = PublishProcessor.create(); + this.pidPropertiesReplay = this.pidProperties.distinctUntilChanged().replay(1); + this.setpoint = PublishProcessor.create(); + this.setpointReplay = this.setpoint.distinctUntilChanged().replay(1); + this.rawOutput = PublishProcessor.create(); + this.rawOutputReplay = this.rawOutput.distinctUntilChanged().replay(1); + this.mode = PublishProcessor.create(); + this.modeReplay = this.mode.distinctUntilChanged().replay(1); + this.sensorDirection = PublishProcessor.create(); + this.sensorDirectionRelay = this.sensorDirection.distinctUntilChanged().replay(1); + + this.modeReplay.connect(); + this.rawOutputReplay.connect(); + this.outputDirectionReplay.connect(); + this.pidPropertiesReplay.connect(); + this.setpointReplay.connect(); + + + this.position = position.replay(1); + this.speed = speed.replay(1); + this.position.connect(); + this.speed.connect(); + } + + public VirtualSpeedController() { + this(Flowable.just(0.0), Flowable.just(0.0)); + } + @Override public Flowable feedback() { return Flowable.empty(); @@ -14,9 +63,13 @@ public void accept(ControllerEvent controllerEvent) { controllerEvent.actOn(this); } + public Flowable outputDirection() { + return outputDirectionReplay; + } + @Override public void setOutputFlipped(boolean flipped) { - + outputDirection.onNext(flipped); } @Override @@ -31,10 +84,54 @@ public boolean revLimitSwitchClosed() { @Override public double position() { - return 0; + return FlowOperators.getLastValue(position); } public double speed() { - return 0; + return FlowOperators.getLastValue(speed); + } + + void changeControlMode(ControlMode mode) { + this.mode.onNext(mode); + } + + public Flowable mode() { + return modeReplay; + } + + void setRawOutput(double output) { + this.rawOutput.onNext(output); + } + + public Flowable rawOutput() { + return rawOutputReplay; + } + + void setLoopProperties(double p, double i, double d, double f) { + pidProperties.onNext(new Double[] {p, i, d, f}); + } + + public Flowable loopProperties() { + return pidPropertiesReplay; + } + + void changeSetpoint(double setpoint) { + this.setpoint.onNext(setpoint); + } + + public Flowable setpoint() { + return setpointReplay; + } + + public void reverseSensor(boolean b) { + this.sensorDirection.onNext(b); + } + + public Flowable sensorDirection() { + return this.sensorDirectionRelay; + } + + public void resetPositionTo(double position) { + } } From e72580a8bc6bc6f93d9e9bf26f6d07b5ab7af104 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Thu, 16 Mar 2017 13:43:35 -0400 Subject: [PATCH 37/45] switching to this method doesn't seem to cause a (or as bad of a) memory leak --- .../controllers/VirtualSpeedController.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index 5913467..e013172 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -1,6 +1,5 @@ package com.nutrons.framework.controllers; -import com.nutrons.framework.util.FlowOperators; import io.reactivex.Flowable; import io.reactivex.flowables.ConnectableFlowable; import io.reactivex.processors.PublishProcessor; @@ -11,8 +10,8 @@ public class VirtualSpeedController extends LoopSpeedController { private final ConnectableFlowable outputDirectionReplay; private final PublishProcessor pidProperties; private final ConnectableFlowable pidPropertiesReplay; - private final ConnectableFlowable position; - private final ConnectableFlowable speed; + private final Flowable position; + private final Flowable speed; private final PublishProcessor setpoint; private final ConnectableFlowable setpointReplay; private final PublishProcessor rawOutput; @@ -21,6 +20,8 @@ public class VirtualSpeedController extends LoopSpeedController { private final ConnectableFlowable modeReplay; private final PublishProcessor sensorDirection; private final ConnectableFlowable sensorDirectionRelay; + private volatile double lastSpeed; + private volatile double lastPosition; public VirtualSpeedController(Flowable position, Flowable speed) { this.outputDirection = PublishProcessor.create(); @@ -42,11 +43,10 @@ public VirtualSpeedController(Flowable position, Flowable speed) this.pidPropertiesReplay.connect(); this.setpointReplay.connect(); - - this.position = position.replay(1); - this.speed = speed.replay(1); - this.position.connect(); - this.speed.connect(); + this.position = position; + this.speed = speed; + this.position.subscribe(x -> this.lastPosition = x); + this.speed.subscribe(x -> this.lastSpeed = x); } public VirtualSpeedController() { @@ -84,11 +84,11 @@ public boolean revLimitSwitchClosed() { @Override public double position() { - return FlowOperators.getLastValue(position); + return lastPosition; } public double speed() { - return FlowOperators.getLastValue(speed); + return lastSpeed; } void changeControlMode(ControlMode mode) { From 3558eeb047b10594d835ea717df4dacd2ecfbb02 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Wed, 22 Mar 2017 17:17:47 -0400 Subject: [PATCH 38/45] Auto triggers more frequently, and uses hot stream --- src/com/nutrons/framework/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/com/nutrons/framework/Robot.java b/src/com/nutrons/framework/Robot.java index 6423377..f7cf552 100644 --- a/src/com/nutrons/framework/Robot.java +++ b/src/com/nutrons/framework/Robot.java @@ -62,9 +62,9 @@ public final Flowable enabledStream() { */ public final Flowable competitionStream() { return Flowable.merge( - FlowOperators.toFlow(this::isAutonomous).filter(x -> x).map((x) -> AUTO), + FlowOperators.toFlow(this::isAutonomous, 50, TimeUnit.MILLISECONDS).filter(x -> x).map((x) -> AUTO), FlowOperators.toFlow(this::isOperatorControl).filter(x -> x).map((x) -> TELE), - FlowOperators.toFlow(this::isTest).filter(x -> x).map((x) -> TEST)).distinctUntilChanged(); + FlowOperators.toFlow(this::isTest).filter(x -> x).map((x) -> TEST)).distinctUntilChanged().replay(1).autoConnect(); // filter(x -> x) will filter all false values from the stream. } @@ -140,4 +140,4 @@ public final boolean isOperatorControl() { public final boolean isNewDataAvailable() { return super.isNewDataAvailable(); } -} \ No newline at end of file +} From eee6cdc2d1564297f886ba0596ed4d037b6a8d9a Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Wed, 22 Mar 2017 18:46:00 -0400 Subject: [PATCH 39/45] fixed imports --- src/com/nutrons/framework/Robot.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/com/nutrons/framework/Robot.java b/src/com/nutrons/framework/Robot.java index f7cf552..1d6c3d5 100644 --- a/src/com/nutrons/framework/Robot.java +++ b/src/com/nutrons/framework/Robot.java @@ -3,14 +3,13 @@ import static com.nutrons.framework.util.CompMode.AUTO; import static com.nutrons.framework.util.CompMode.TELE; import static com.nutrons.framework.util.CompMode.TEST; -import static io.reactivex.Flowable.combineLatest; import com.nutrons.framework.commands.Command; import com.nutrons.framework.util.CompMode; import com.nutrons.framework.util.FlowOperators; import edu.wpi.first.wpilibj.SampleRobot; import io.reactivex.Flowable; -import io.reactivex.schedulers.Schedulers; +import java.util.concurrent.TimeUnit; public abstract class Robot extends SampleRobot { From 2b96166c4ae4108e847295cfacdf9399ed6bff0d Mon Sep 17 00:00:00 2001 From: abstractedowl Date: Wed, 22 Mar 2017 19:54:04 -0400 Subject: [PATCH 40/45] Added current getter implementation --- .../nutrons/framework/controllers/LoopSpeedController.java | 2 ++ src/com/nutrons/framework/controllers/Talon.java | 4 ++++ .../framework/controllers/VirtualSpeedController.java | 5 +++++ 3 files changed, 11 insertions(+) diff --git a/src/com/nutrons/framework/controllers/LoopSpeedController.java b/src/com/nutrons/framework/controllers/LoopSpeedController.java index c4a6b38..07080a8 100644 --- a/src/com/nutrons/framework/controllers/LoopSpeedController.java +++ b/src/com/nutrons/framework/controllers/LoopSpeedController.java @@ -8,6 +8,8 @@ */ public abstract class LoopSpeedController implements Consumer { + public abstract double getCurrent(); + public abstract Flowable feedback(); @Override diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index c6c4585..e469f9b 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -69,6 +69,10 @@ void changeSetpoint(double setpoint) { } + public double getCurrent() { + return this.talon.getOutputCurrent(); + } + void setP(double pval) { this.talon.setP(pval); } diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index e013172..bb18ae2 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -53,6 +53,11 @@ public VirtualSpeedController() { this(Flowable.just(0.0), Flowable.just(0.0)); } + @Override + public double getCurrent() { + return 0; + } + @Override public Flowable feedback() { return Flowable.empty(); From 2f48746459af565b22d607343145e1547aa47f49 Mon Sep 17 00:00:00 2001 From: abstractedowl Date: Wed, 22 Mar 2017 19:55:05 -0400 Subject: [PATCH 41/45] Committed FRamework submodule --- FRamework.iml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 FRamework.iml diff --git a/FRamework.iml b/FRamework.iml new file mode 100644 index 0000000..5f4ccd1 --- /dev/null +++ b/FRamework.iml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file From 63d85dba32828c41f2407d1ec1d850cc4f6dbf08 Mon Sep 17 00:00:00 2001 From: nutronsds Date: Tue, 28 Mar 2017 18:31:28 -0400 Subject: [PATCH 42/45] added ramping - untested --- .../nutrons/framework/controllers/LoopSpeedController.java | 2 ++ src/com/nutrons/framework/controllers/Talon.java | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/src/com/nutrons/framework/controllers/LoopSpeedController.java b/src/com/nutrons/framework/controllers/LoopSpeedController.java index 07080a8..5e1d16c 100644 --- a/src/com/nutrons/framework/controllers/LoopSpeedController.java +++ b/src/com/nutrons/framework/controllers/LoopSpeedController.java @@ -48,4 +48,6 @@ public void setReversedSensor(boolean flipped) { public abstract boolean revLimitSwitchClosed(); public abstract double position(); + + public abstract void setVoltageRampRate(double v); } diff --git a/src/com/nutrons/framework/controllers/Talon.java b/src/com/nutrons/framework/controllers/Talon.java index e469f9b..e177d4c 100644 --- a/src/com/nutrons/framework/controllers/Talon.java +++ b/src/com/nutrons/framework/controllers/Talon.java @@ -3,6 +3,7 @@ import static com.nutrons.framework.util.FlowOperators.toFlow; import com.ctre.CANTalon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import io.reactivex.Flowable; import java.security.InvalidParameterException; @@ -42,6 +43,7 @@ public Talon(int port, Talon toFollow) { } void set(double value) { + SmartDashboard.putNumber("talon " + this.id(), value); this.talon.set(value); } @@ -149,4 +151,9 @@ public boolean revLimitSwitchClosed() { public double getClosedLoopError() { return this.talon.getClosedLoopError(); } + + @Override + public void setVoltageRampRate(double v){ + this.talon.setVoltageRampRate(v); + } } From 1a60b4e9c078e121c700f878287b1a4d3f064407 Mon Sep 17 00:00:00 2001 From: Asher Gottlieb Date: Wed, 29 Mar 2017 18:42:54 -0400 Subject: [PATCH 43/45] implemented voltage method in virtual speed controller --- .../framework/controllers/VirtualSpeedController.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/com/nutrons/framework/controllers/VirtualSpeedController.java b/src/com/nutrons/framework/controllers/VirtualSpeedController.java index bb18ae2..ca88ae0 100644 --- a/src/com/nutrons/framework/controllers/VirtualSpeedController.java +++ b/src/com/nutrons/framework/controllers/VirtualSpeedController.java @@ -92,6 +92,11 @@ public double position() { return lastPosition; } + @Override + public void setVoltageRampRate(double v) { + + } + public double speed() { return lastSpeed; } From 484c312bae0962e76e37a9e3788f7b9a721b2e20 Mon Sep 17 00:00:00 2001 From: AndreasLc1103 Date: Tue, 18 Apr 2017 17:38:57 -0400 Subject: [PATCH 44/45] The deadband reassign stuffs i forgot to push to a branch --- 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 b305e43..4c2b6d9 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -4,6 +4,7 @@ import static java.lang.Math.max; import static java.lang.Math.min; +import com.nutrons.framework.inputs.CommonController; import io.reactivex.Flowable; import io.reactivex.FlowableTransformer; import io.reactivex.disposables.CompositeDisposable; @@ -57,12 +58,24 @@ public static Flowable deadband(Flowable input) { /** * Creates a function that will return the input value, unless that value is within the range * specified by minimum and maximum. If so, the value will be changed to remap. + * 0 if in deadband else f(x-d) */ public static Function deadbandMap(double minimum, double maximum, double remap) { return bandMap(minimum, maximum, x -> remap); } - + public static Function deadbandAssign( double min , double max, double remap) { + return (Double x) -> { + if (x <= min) { + return x - min; + } else if (x >= max) + { + return x + max;} + else{ + return remap; + } + }; + } /** * Creates a function that will return the input value, unless that value is within the range * specified by minimum and maximum. If so, the value will be passed through the remap function. From ad3c6225fb7b8bf9fa40cee249090cc488408889 Mon Sep 17 00:00:00 2001 From: cappuccinocat Date: Tue, 18 Apr 2017 21:15:55 -0400 Subject: [PATCH 45/45] fixed deadbandassign --- .../nutrons/framework/util/FlowOperators.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/com/nutrons/framework/util/FlowOperators.java b/src/com/nutrons/framework/util/FlowOperators.java index 4c2b6d9..1c85a8a 100644 --- a/src/com/nutrons/framework/util/FlowOperators.java +++ b/src/com/nutrons/framework/util/FlowOperators.java @@ -4,7 +4,6 @@ import static java.lang.Math.max; import static java.lang.Math.min; -import com.nutrons.framework.inputs.CommonController; import io.reactivex.Flowable; import io.reactivex.FlowableTransformer; import io.reactivex.disposables.CompositeDisposable; @@ -64,18 +63,19 @@ public static Function deadbandMap(double minimum, double maximu double remap) { return bandMap(minimum, maximum, x -> remap); } - public static Function deadbandAssign( double min , double max, double remap) { + + public static Function deadbandAssign(double deadzone, double rampRate) { return (Double x) -> { - if (x <= min) { - return x - min; - } else if (x >= max) - { - return x + max;} - else{ - return remap; + if (x <= -(deadzone / 2)) { + return -Math.pow(Math.abs(x + deadzone / 2) / (1 - deadzone / 2), rampRate); + } else if (x >= (deadzone / 2)) { + return Math.pow(x - deadzone / 2, rampRate) / (1 - deadzone / 2); + } else { + return 0.0; } }; } + /** * Creates a function that will return the input value, unless that value is within the range * specified by minimum and maximum. If so, the value will be passed through the remap function.