Skip to content

Commit

Permalink
COMMON-213: Tests working
Browse files Browse the repository at this point in the history
  • Loading branch information
@minhn02
minhn02 committed Feb 24, 2019
1 parent 30697eb commit d3162e8
Show file tree
Hide file tree
Showing 5 changed files with 81 additions and 59 deletions.
13 changes: 8 additions & 5 deletions common-wpi-util/src/main/java/com/team2073/common/mediator/Mediator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ public void registerConflict(Conflict... conflicts) {
if (conflictMap.get(conflict.getOriginSubsystem()) == null) {
ArrayList<Conflict> originConflicts = new ArrayList<>();
conflictMap.put(conflict.getOriginSubsystem(), originConflicts);
logger.debug("Creating conflict list for: [{}] and [{}]", conflict.getOriginSubsystem());
logger.debug("Creating conflict list for: [{}] and [{}]", conflict.getOriginSubsystem(), conflict.getConflictingSubsystem());
}
if (conflictMap.get(conflict.getConflictingSubsystem()) == null) {
ArrayList<Conflict> inverseConflicts = new ArrayList<>();
Expand Down Expand Up @@ -156,16 +156,19 @@ private void execute(Request request) {
Condition condition = request.getCondition();
Class subsystem = request.getSubsystem();

if (conflicts.isEmpty()) {
logger.debug("Executing request [{}]. Conflicts: [{}]", request.getName(), conflicts.size());

subsystemMap.get(subsystem).set(condition.getConditionValue());
if (conflicts.isEmpty() || request.getParallelConflict() != null) {
if (request.getParallelConflict() != null) {
Conflict parallelConflict = request.getParallelConflict();
logger.debug("Executing parallel Conflict [{}]", parallelConflict.getName());
subsystemMap.get(parallelConflict.getOriginSubsystem()).set(parallelConflict.getOriginParallelResolution(subsystemMap.get(parallelConflict.getOriginSubsystem()),
subsystemMap.get(parallelConflict.getConflictingSubsystem())).getConditionValue());


System.out.println("parallel conflict output: " + parallelConflict.getOriginParallelResolution(subsystemMap.get(parallelConflict.getOriginSubsystem()),
subsystemMap.get(parallelConflict.getConflictingSubsystem())).getConditionValue());
}
logger.debug("Executing request [{}]. Conflicts: [{}]", request.getName(), conflicts.size());
subsystemMap.get(subsystem).set(condition.getConditionValue());
} else {
for (Conflict conflict : conflicts) {
logger.debug("Conflict [{}]", conflict.toString());
Expand Down
14 changes: 11 additions & 3 deletions common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/PositionBasedConflict.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ class PositionBasedConflict(

}

fun isWithinBounds(lowerBound: Double, value: Double, upperBound: Double): Boolean{
return Range.between(lowerBound, upperBound).contains(value)
}

override fun getOriginParallelResolution(originSubsystem: ColleagueSubsystem<Double>, conflictingSubsystem: ColleagueSubsystem<Double>): Condition<Double> {
val originPoint: Vector2D = (originSubsystem as PositionBasedSubsystem).positionToPoint(originSubsystem.getCurrentCondition().getConditionValue())
val conflictingPoint: Vector2D = (conflictingSubsystem as PositionBasedSubsystem).positionToPoint(conflictingSubsystem.getCurrentCondition().getConditionValue())
Expand All @@ -42,7 +46,6 @@ class PositionBasedConflict(
} else {
conflictingPoint.y + conflictingSafetyRange + originSubsystem.getSafetyRange()
}

nearestOriginSafeX = if(originPoint.x > conflictingPoint.x){
conflictingPoint.x - conflictingSafetyRange - originSubsystem.getSafetyRange()
}else{
Expand All @@ -52,8 +55,13 @@ class PositionBasedConflict(
val nearestOriginSafePoint = Vector2D(nearestOriginSafeX, nearestOriginSafeY)
val nearestOriginSafePosition = originSubsystem.pointToPosition(nearestOriginSafePoint)

return PositionBasedCondition(nearestOriginSafePosition,
Range.between(nearestOriginSafePosition - originSafetyRange, nearestOriginSafePosition + originSafetyRange))
//TODO needs to convert safety range to a relative point
return if(originPoint.x - conflictingPoint.x < originSafetyRange || originPoint.y - conflictingPoint.y < originSafetyRange){
originSubsystem.getCurrentCondition()
}else {
PositionBasedCondition(nearestOriginSafePosition,
Range.between(nearestOriginSafePosition - originSafetyRange, nearestOriginSafePosition + originSafetyRange))
}
}

override fun isRequestConflicting(request: Request<Double>, currentConflictingCondition: Condition<Double>, currentOriginCondition: Condition<Double>): Boolean {
Expand Down
4 changes: 0 additions & 4 deletions common-wpi-util/src/main/java/com/team2073/common/mediator/subsys/PositionBasedSubsystem.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,6 @@ interface PositionBasedSubsystem : ColleagueSubsystem<Double> {

fun getSafetyRange(): Double

fun getLowerLeftBound(): Vector2D

fun getUpperRightBound(): Vector2D

fun positionToPoint(position: Double): Vector2D

fun pointToPosition(point: Vector2D): Double
Expand Down
79 changes: 49 additions & 30 deletions common-wpi-util/src/test/java/com/team2073/common/mediator/MediatorTestFixtures.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ public void set(Double place) {

@Override
public double getSafetyRange() {
return 0;
return 5;
}

@Override
Expand All @@ -33,6 +33,8 @@ public void onPeriodic() {
return;
}

System.out.println("linear: " + currentPosition);

if(!currentPosition.equals(setpoint)) {
if (currentPosition > setpoint) {
currentPosition--;
Expand All @@ -59,21 +61,60 @@ public Vector2D positionToPoint(double position) {
public double pointToPosition(@NotNull Vector2D point) {
return point.getY();
}
}

static class LinearPositionSubsystem2 implements PositionBasedSubsystem {

private Double setpoint;
private Double currentPosition = 0d;

@Override
public void set(Double place) {
setpoint = place;
}

@Override
public double getSafetyRange() {
return 5;
}

@Override
public void onPeriodic() {
if (setpoint == null) {
return;
}

System.out.println("linear2: " + currentPosition);

if (!currentPosition.equals(setpoint)) {
if (currentPosition > setpoint) {
currentPosition--;
} else {
currentPosition++;
}
}

}

@NotNull
@Override
public Vector2D getLowerLeftBound() {
return new Vector2D(0, 0);
public Condition<Double> getCurrentCondition() {
return new PositionBasedCondition(currentPosition, Range.between(currentPosition - getSafetyRange(), currentPosition + getSafetyRange()));
}

@NotNull
@Override
public Vector2D getUpperRightBound() {
return new Vector2D(0,100);
public Vector2D positionToPoint(double position) {
return new Vector2D(0, currentPosition);
}

@Override
public double pointToPosition(@NotNull Vector2D point) {
return point.getY();
}
}

static class HorizontalPositionSubsystem implements PositionBasedSubsystem {
static class HorizontalPositionSubsystem implements PositionBasedSubsystem {

private Double setpoint;
private Double currentPosition = 0d;
Expand All @@ -95,6 +136,8 @@ public void onPeriodic() {
return;
}

System.out.println("horizontal: " + currentPosition);

if(!setpoint.equals(currentPosition)) {
if (currentPosition > setpoint) {
currentPosition--;
Expand All @@ -120,18 +163,6 @@ public Vector2D positionToPoint(double position) {
public double pointToPosition(@NotNull Vector2D point) {
return point.getX();
}

@NotNull
@Override
public Vector2D getLowerLeftBound() {
return new Vector2D(0, 0);
}

@NotNull
@Override
public Vector2D getUpperRightBound() {
return new Vector2D(100, 0);
}
}

static class DeadPositionSubsystem implements PositionBasedSubsystem {
Expand Down Expand Up @@ -166,18 +197,6 @@ public Vector2D positionToPoint(double position) {
public double pointToPosition(@NotNull Vector2D point) {
return 0;
}

@NotNull
@Override
public Vector2D getLowerLeftBound() {
return new Vector2D(0, 0);
}

@NotNull
@Override
public Vector2D getUpperRightBound() {
return new Vector2D(0, 0);
}
}

enum State implements SubsystemStateCondition<State> {
Expand Down
30 changes: 13 additions & 17 deletions common-wpi-util/src/test/java/com/team2073/common/mediator/MediatorTests.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@

class MediatorTests {
private LinearPositionSubsystem linearPositionSubsystem = new LinearPositionSubsystem();
private LinearPositionSubsystem2 linearPositionSubsystem2 = new LinearPositionSubsystem2();
private HorizontalPositionSubsystem horizontalPositionSubsystem = new HorizontalPositionSubsystem();
private StateSubsystem stateSubsystem = new StateSubsystem();
private StateSubsystemDeux stateSubsystemDeux = new StateSubsystemDeux();
Expand Down Expand Up @@ -176,11 +177,6 @@ public void mediator_WHEN_FoundConflict_SHOULD_AddRequest() {
TestMediator testMediator = new TestMediator();
testMediator.registerColleague(stateSubsystem);
testMediator.registerColleague(linearPositionSubsystem);
testMediator.registerConflict(new PositionStateBasedConflict(LinearPositionSubsystem.class,
new PositionBasedCondition(90, Range.between(85d, 95d)),
MediatorTestFixtures.StateSubsystem.class,
new StateBasedCondition(State.OPEN),
State.CLOSE, true, false));
testMediator.registerConflict(new StatePositionBasedConflict(StateSubsystem.class,
new StateBasedCondition(State.OPEN),
LinearPositionSubsystem.class,
Expand All @@ -189,12 +185,12 @@ public void mediator_WHEN_FoundConflict_SHOULD_AddRequest() {
Request request = new Request<>(LinearPositionSubsystem.class, new PositionBasedCondition(90, Range.between(85d, 95d)));
Request request2 = new Request<>(StateSubsystem.class, new StateBasedCondition(State.OPEN));

testMediator.add(request);
testMediator.add(request2);
testMediator.add(request);
Queue<Deque<Request>> list = testMediator.getExecuteList();
assertThat(list.size()).isEqualTo(2);

callPeriodic(1, testMediator, linearPositionSubsystem);
callPeriodic(4, testMediator, linearPositionSubsystem);

System.out.println(Arrays.toString(testMediator.getExecuteList().toArray()));

Expand All @@ -210,11 +206,11 @@ public void mediator_WHEN_EncounteredConflict_SHOULD_ResolveConflict() {
new PositionBasedCondition(90, Range.between(85d, 95d)),
MediatorTestFixtures.StateSubsystem.class,
new StateBasedCondition(State.OPEN),
State.CLOSE, true, false));
State.CLOSE, false, false));
testMediator.registerConflict(new StatePositionBasedConflict(StateSubsystem.class,
new StateBasedCondition(State.OPEN),
LinearPositionSubsystem.class,
new PositionBasedCondition(90, Range.between(85d, 95d)), State.CLOSE, true, false));
new PositionBasedCondition(90, Range.between(85d, 95d)), State.CLOSE, false, false));

Request positionRequest = new Request<>(LinearPositionSubsystem.class, new PositionBasedCondition(90, Range.between(85d, 95d)));
Request stateRequest = new Request<State>(StateSubsystem.class, new StateBasedCondition(State.OPEN));
Expand Down Expand Up @@ -293,19 +289,19 @@ public void mediator_WHEN_RequestTakesTooLong_SHOULD_ClearOutRequest() {
@Test
public void mediator_SHOULD_HandlePositionBasedConflict_WITH_ParallelActions() {
TestMediator testMediator = new TestMediator();
testMediator.registerColleague(linearPositionSubsystem, horizontalPositionSubsystem);
testMediator.registerConflict(new PositionBasedConflict(HorizontalPositionSubsystem.class,
testMediator.registerColleague(linearPositionSubsystem, linearPositionSubsystem2);
testMediator.registerConflict(new PositionBasedConflict(LinearPositionSubsystem.class,
new PositionBasedCondition(10, Range.between(5d, 20d)),
LinearPositionSubsystem.class,
new PositionBasedCondition(10, Range.between(5d, 20d)), false, true));
LinearPositionSubsystem2.class,
new PositionBasedCondition(10, Range.between(5d, 20d)), true, true));

testMediator.add(new Request<>(HorizontalPositionSubsystem.class, new PositionBasedCondition(10d, Range.between(10d, 10d))));
testMediator.add(new Request<>(LinearPositionSubsystem.class, new PositionBasedCondition(10d, Range.between(10d, 10d))));
testMediator.add(new Request<>(LinearPositionSubsystem2.class, new PositionBasedCondition(10d, Range.between(10d, 10d))));

callPeriodic(20, testMediator, horizontalPositionSubsystem, linearPositionSubsystem);
callPeriodic(50, testMediator, linearPositionSubsystem2, linearPositionSubsystem);

assertThat(horizontalPositionSubsystem.getCurrentCondition().getConditionValue()).isEqualTo(10d);
assertThat(linearPositionSubsystem.getCurrentCondition().getConditionValue()).isEqualTo(20d);
assertThat(linearPositionSubsystem2.getCurrentCondition().getConditionValue()).isNotZero();
assertThat(linearPositionSubsystem.getCurrentCondition().getConditionValue()).isNotZero();

}

Expand Down

0 comments on commit d3162e8

Please sign in to comment.