diff --git a/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt b/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt index f3d8059..5874aa7 100644 --- a/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt +++ b/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt @@ -99,12 +99,22 @@ object Auto : RobotLifecycle { TurnToAngle(-SCALE_TO_SWITCH_TURN) and SendToSwitch() then findAndDeliverCube - AutoMode.LEFT_TO_RIGHT_SCALE -> LeftToRightScale() and SendToSwitch() and + AutoMode.LEFT_TO_RIGHT_SCALE_PICKUP -> LeftToRightScale() and SendToSwitch() and + SendToScale().waitUntil(isReadyToSendToScale) then + EjectBlock() then TurnToAngle(-SCALE_TO_SWITCH_TURN) and SendToSwitch() then + findCube + + AutoMode.RIGHT_TO_LEFT_SCALE_PICKUP -> RightToLeftScale() and SendToSwitch() and + SendToScale().waitUntil(isReadyToSendToScale) then + EjectBlock() then TurnToAngle(SCALE_TO_SWITCH_TURN) and SendToSwitch() then + findCube + + AutoMode.LEFT_TO_RIGHT_SCALE_SWITCH -> LeftToRightScale() and SendToSwitch() and SendToScale().waitUntil(isReadyToSendToScale) then EjectBlock() then TurnToAngle(-SCALE_TO_SWITCH_TURN) and SendToSwitch() then findAndDeliverCube - AutoMode.RIGHT_TO_LEFT_SCALE -> RightToLeftScale() and SendToSwitch() and + AutoMode.RIGHT_TO_LEFT_SCALE_SWITCH -> RightToLeftScale() and SendToSwitch() and SendToScale().waitUntil(isReadyToSendToScale) then EjectBlock() then TurnToAngle(SCALE_TO_SWITCH_TURN) and SendToSwitch() then findAndDeliverCube diff --git a/src/main/java/org/sert2521/powerup/util/Modes.kt b/src/main/java/org/sert2521/powerup/util/Modes.kt index 5793995..65e6f4e 100644 --- a/src/main/java/org/sert2521/powerup/util/Modes.kt +++ b/src/main/java/org/sert2521/powerup/util/Modes.kt @@ -9,10 +9,12 @@ import org.sert2521.powerup.util.AutoMode.End import org.sert2521.powerup.util.AutoMode.LEFT_TO_LEFT_SCALE_PICKUP import org.sert2521.powerup.util.AutoMode.LEFT_TO_LEFT_SCALE_SWITCH import org.sert2521.powerup.util.AutoMode.LEFT_TO_LEFT_SWITCH -import org.sert2521.powerup.util.AutoMode.LEFT_TO_RIGHT_SCALE +import org.sert2521.powerup.util.AutoMode.LEFT_TO_RIGHT_SCALE_PICKUP +import org.sert2521.powerup.util.AutoMode.LEFT_TO_RIGHT_SCALE_SWITCH import org.sert2521.powerup.util.AutoMode.MIDDLE_TO_LEFT_SWITCH import org.sert2521.powerup.util.AutoMode.MIDDLE_TO_RIGHT_SWITCH -import org.sert2521.powerup.util.AutoMode.RIGHT_TO_LEFT_SCALE +import org.sert2521.powerup.util.AutoMode.RIGHT_TO_LEFT_SCALE_PICKUP +import org.sert2521.powerup.util.AutoMode.RIGHT_TO_LEFT_SCALE_SWITCH import org.sert2521.powerup.util.AutoMode.RIGHT_TO_RIGHT_SCALE_PICKUP import org.sert2521.powerup.util.AutoMode.RIGHT_TO_RIGHT_SCALE_SWITCH import org.sert2521.powerup.util.AutoMode.RIGHT_TO_RIGHT_SWITCH @@ -48,11 +50,19 @@ val autoMode: AutoMode End.SCALE -> LEFT_TO_LEFT_SCALE_SWITCH else -> error("Unknown mode: $endChoice") } - OwnedSide.RIGHT -> RIGHT_TO_RIGHT_SWITCH + OwnedSide.RIGHT -> when (endChoice) { + End.SWITCH -> LEFT_TO_LEFT_SWITCH + End.SCALE -> LEFT_TO_RIGHT_SCALE_PICKUP + else -> error("Unknown mode: $endChoice") + } else -> error("Impossible condition: $scaleSide") } Start.RIGHT -> when (scaleSide) { - OwnedSide.LEFT -> RIGHT_TO_LEFT_SCALE + OwnedSide.LEFT -> when (endChoice) { + End.SWITCH -> RIGHT_TO_LEFT_SCALE_PICKUP + End.SCALE -> RIGHT_TO_LEFT_SCALE_SWITCH + else -> error("Unknown mode: $endChoice") + } OwnedSide.RIGHT -> RIGHT_TO_RIGHT_SCALE_PICKUP else -> error("Impossible condition: $scaleSide") } @@ -61,11 +71,19 @@ val autoMode: AutoMode Start.MIDDLE -> MIDDLE_TO_RIGHT_SWITCH Start.LEFT -> when (scaleSide) { OwnedSide.LEFT -> LEFT_TO_LEFT_SCALE_PICKUP - OwnedSide.RIGHT -> LEFT_TO_RIGHT_SCALE + OwnedSide.RIGHT -> when (endChoice) { + End.SWITCH -> LEFT_TO_RIGHT_SCALE_PICKUP + End.SCALE -> LEFT_TO_RIGHT_SCALE_SWITCH + else -> error("Unknown mode: $endChoice") + } else -> error("Impossible condition: $scaleSide") } Start.RIGHT -> when (scaleSide) { - OwnedSide.LEFT -> RIGHT_TO_RIGHT_SWITCH + OwnedSide.LEFT -> when (endChoice) { + End.SWITCH -> RIGHT_TO_RIGHT_SWITCH + End.SCALE -> RIGHT_TO_LEFT_SCALE_PICKUP + else -> error("Unknown mode: $endChoice") + } OwnedSide.RIGHT -> when (endChoice) { End.SWITCH -> RIGHT_TO_RIGHT_SWITCH End.SCALE -> RIGHT_TO_RIGHT_SCALE_SWITCH @@ -91,8 +109,10 @@ enum class AutoMode { CROSS_BASELINE, LEFT_TO_LEFT_SWITCH, RIGHT_TO_RIGHT_SWITCH, MIDDLE_TO_LEFT_SWITCH, MIDDLE_TO_RIGHT_SWITCH, - LEFT_TO_LEFT_SCALE_PICKUP, LEFT_TO_LEFT_SCALE_SWITCH, LEFT_TO_RIGHT_SCALE, - RIGHT_TO_RIGHT_SCALE_PICKUP, RIGHT_TO_RIGHT_SCALE_SWITCH, RIGHT_TO_LEFT_SCALE, + LEFT_TO_LEFT_SCALE_PICKUP, LEFT_TO_LEFT_SCALE_SWITCH, + LEFT_TO_RIGHT_SCALE_PICKUP, LEFT_TO_RIGHT_SCALE_SWITCH, + RIGHT_TO_RIGHT_SCALE_PICKUP, RIGHT_TO_RIGHT_SCALE_SWITCH, + RIGHT_TO_LEFT_SCALE_PICKUP, RIGHT_TO_LEFT_SCALE_SWITCH, TEST_LEFT, TEST_RIGHT; enum class Start { @@ -121,8 +141,10 @@ object Modes : RobotLifecycle { "Right to Right Scale with Pickup" to RIGHT_TO_RIGHT_SCALE_PICKUP, "Left to Left Scale with Switch" to LEFT_TO_LEFT_SCALE_SWITCH, "Right to Right Scale with Switch" to RIGHT_TO_RIGHT_SCALE_SWITCH, - "Left to Right Scale" to LEFT_TO_RIGHT_SCALE, - "Right to Left Scale" to RIGHT_TO_LEFT_SCALE, + "Left to Right Scale with Pickup" to LEFT_TO_RIGHT_SCALE_PICKUP, + "Right to Left Scale with Pickup" to RIGHT_TO_LEFT_SCALE_PICKUP, + "Left to Right Scale with Switch" to LEFT_TO_RIGHT_SCALE_SWITCH, + "Right to Left Scale with Switch" to RIGHT_TO_LEFT_SCALE_SWITCH, "Test Left" to TEST_LEFT, "Test Right" to TEST_RIGHT