PositionState checks a range instead of just the requested position

team-2073-eagleforce · Feb 19, 2019 · 7e9a4f5 · 7e9a4f5
1 parent 9a761b4
commit 7e9a4f5
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 22 deletions.
diff --git a/common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/Conflict.kt b/common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/Conflict.kt
@@ -12,7 +12,7 @@ abstract class Conflict<OT, CT>(
 
     abstract fun invert(): Conflict<CT, OT>
 
-    abstract fun isRequestConflicting(request: Request<OT>, conflictingCondition: Condition<CT>, currentOriginCondition: Condition<OT>): Boolean
+    abstract fun isRequestConflicting(request: Request<OT>, currentConflictingCondition: Condition<CT>, currentOriginCondition: Condition<OT>): Boolean
 
     abstract fun isConditionConflicting(originCondition: Condition<OT>, conflictingCondition: Condition<CT>): Boolean
 

diff --git a/common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/PositionBasedConflict.kt b/common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/PositionBasedConflict.kt
@@ -26,10 +26,10 @@ class PositionBasedConflict(
 
     }
 
-    override fun isRequestConflicting(request: Request<Double>, conflictingCondition: Condition<Double>, currentOriginCondition: Condition<Double>): Boolean {
-        var range: Range<Double> = Range.between(currentOriginCondition.getConditionValue(), request.condition.getConditionValue())
-        var conflictingRange: Range<Double> = Range.between((conflictingCondition as PositionBasedCondition).range.minimum,
-                conflictingCondition.range.maximum)
+    override fun isRequestConflicting(request: Request<Double>, currentConflictingCondition: Condition<Double>, currentOriginCondition: Condition<Double>): Boolean {
+        val range: Range<Double> = Range.between(currentOriginCondition.getConditionValue(), request.condition.getConditionValue())
+        val conflictingRange: Range<Double> = Range.between((currentConflictingCondition as PositionBasedCondition).range.minimum,
+                currentConflictingCondition.range.maximum)
 
         return range.isOverlappedBy(conflictingRange)
     }

diff --git a/...pi-util/src/main/java/com/team2073/common/mediator/conflict/PositionStateBasedConflict.kt b/...pi-util/src/main/java/com/team2073/common/mediator/conflict/PositionStateBasedConflict.kt
@@ -5,6 +5,7 @@ import com.team2073.common.mediator.condition.StateBasedCondition
 import com.team2073.common.mediator.request.Request
 import com.team2073.common.mediator.subsys.ColleagueSubsystem
 import com.team2073.common.mediator.subsys.SubsystemStateCondition
+import org.apache.commons.lang3.Range
 
 class PositionStateBasedConflict<T : Enum<T>>(
         val originSubsystemPS: Class<out ColleagueSubsystem<Double>>,
@@ -22,9 +23,10 @@ class PositionStateBasedConflict<T : Enum<T>>(
         return StateBasedCondition(resolveState)
     }
 
-    override fun isRequestConflicting(request: Request<Double>, conflictingCondition: Condition<SubsystemStateCondition<T>>, currentOriginCondition: Condition<Double>): Boolean {
-        return conflictingCondition.isInCondition(conflictingConditionPS)
-                && originCondition.isInCondition(request.condition)
+    override fun isRequestConflicting(request: Request<Double>, currentConflictingCondition: Condition<SubsystemStateCondition<T>>, currentOriginCondition: Condition<Double>): Boolean {
+        val travelRange: Range<Double> = Range.between(currentOriginCondition.getConditionValue(), request.condition.getConditionValue())
+        return currentConflictingCondition.isInCondition(conflictingConditionPS)
+                && travelRange.contains(originConditionPS.getConditionValue())
     }
 
     override fun invert(): Conflict<SubsystemStateCondition<T>, Double> {

diff --git a/common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/StateBasedConflict.kt b/common-wpi-util/src/main/java/com/team2073/common/mediator/conflict/StateBasedConflict.kt
@@ -19,8 +19,8 @@ class StateBasedConflict<OT : Enum<OT>, CT : Enum<CT>>(
         return StateBasedConflict(conflictingSubsystemS, conflictingConditionS, originSubsystemS, originConditionS, inverseResolveState, resolveState)
     }
 
-    override fun isRequestConflicting(request: Request<SubsystemStateCondition<OT>>, conflictingCondition: Condition<SubsystemStateCondition<CT>>, currentOriginCondition: Condition<SubsystemStateCondition<OT>>): Boolean {
-        return originCondition.isInCondition(request.condition) && conflictingCondition.isInCondition(conflictingConditionS)
+    override fun isRequestConflicting(request: Request<SubsystemStateCondition<OT>>, currentConflictingCondition: Condition<SubsystemStateCondition<CT>>, currentOriginCondition: Condition<SubsystemStateCondition<OT>>): Boolean {
+        return originCondition.isInCondition(request.condition) && currentConflictingCondition.isInCondition(conflictingConditionS)
     }
 
     override fun isConditionConflicting(originCondition: Condition<SubsystemStateCondition<OT>>, conflictingCondition: Condition<SubsystemStateCondition<CT>>): Boolean {

diff --git a/...pi-util/src/main/java/com/team2073/common/mediator/conflict/StatePositionBasedConflict.kt b/...pi-util/src/main/java/com/team2073/common/mediator/conflict/StatePositionBasedConflict.kt
@@ -27,18 +27,8 @@ class StatePositionBasedConflict<T : Enum<T>>(val originSubsystemPS: Class<out C
 
     }
 
-    override fun isRequestConflicting(request: Request<SubsystemStateCondition<T>>, conflictingCondition: Condition<Double>, currentOriginCondition: Condition<SubsystemStateCondition<T>>): Boolean {
-        var conflictCase = false
-        var originCase = false
-
-        if (conflictingCondition.isInCondition(conflictingConditionPS)) {
-            conflictCase = true
-        }
-        if (originCondition.isInCondition(request.condition)) {
-            originCase = true
-        }
-
-        return originCase && conflictCase
+    override fun isRequestConflicting(request: Request<SubsystemStateCondition<T>>, currentConflictingCondition: Condition<Double>, currentOriginCondition: Condition<SubsystemStateCondition<T>>): Boolean {
+        return request.condition.isInCondition(originCondition) && currentConflictingCondition.isInCondition(conflictingConditionPS)
     }
 
     override fun invert(): Conflict<Double, SubsystemStateCondition<T>> {