From cf5db220202fa71f6e4214804c1e93c35afef1fd Mon Sep 17 00:00:00 2001 From: blueberry Date: Thu, 29 Feb 2024 16:23:53 -0800 Subject: [PATCH] refactored alliance choosing --- .../team449/control/auto/ChoreoTrajectory.kt | 8 +++++--- .../auto/routines/Experimental3PieceMid.kt | 15 ++++++--------- .../robot2024/auto/routines/FivePiece.kt | 15 ++++++--------- .../auto/routines/FivePieceSubwoofer.kt | 15 ++++++--------- .../auto/routines/FourPieceSubwooferAmp.kt | 15 ++++++--------- .../auto/routines/FourPieceSubwooferStage.kt | 15 ++++++--------- .../robot2024/auto/routines/RoutineChooser.kt | 17 +++++++++-------- 7 files changed, 44 insertions(+), 56 deletions(-) diff --git a/src/main/kotlin/frc/team449/control/auto/ChoreoTrajectory.kt b/src/main/kotlin/frc/team449/control/auto/ChoreoTrajectory.kt index d354959..8fa14f4 100644 --- a/src/main/kotlin/frc/team449/control/auto/ChoreoTrajectory.kt +++ b/src/main/kotlin/frc/team449/control/auto/ChoreoTrajectory.kt @@ -8,7 +8,9 @@ import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.numbers.N2 import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.wpilibj.DriverStation.Alliance import edu.wpi.first.wpilibj.Filesystem +import frc.team449.robot2024.auto.AutoUtil import org.json.simple.JSONArray import org.json.simple.JSONObject import org.json.simple.parser.JSONParser @@ -67,7 +69,8 @@ class ChoreoTrajectory( * @param filename Filename of .chor document file. By default, it will check in the deploy/choreo folder */ fun createTrajectory( - filename: String + filename: String, + alliance: Alliance = Alliance.Blue ): MutableList { val path = Filesystem.getDeployDirectory().absolutePath.plus("/choreo/$filename.chor") val document = (JSONParser().parse(FileReader(File(path).absolutePath)) as JSONObject)["paths"] as HashMap<*, *> @@ -93,8 +96,7 @@ class ChoreoTrajectory( ) ) } - - return trajList + return if (alliance == Alliance.Red) AutoUtil.transformForRed(trajList) else trajList } private fun deadband(value: Double, deadband: Double = 1e-6): Double { diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental3PieceMid.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental3PieceMid.kt index 93dfb38..f481a98 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental3PieceMid.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental3PieceMid.kt @@ -1,5 +1,6 @@ package frc.team449.robot2024.auto.routines +import edu.wpi.first.wpilibj.DriverStation.Alliance import frc.team449.control.auto.ChoreoRoutine import frc.team449.control.auto.ChoreoRoutineStructure import frc.team449.control.auto.ChoreoTrajectory @@ -8,7 +9,7 @@ import frc.team449.robot2024.auto.AutoUtil class Experimental3PieceMid( robot: Robot, - isRed: Boolean + alliance: Alliance ) : ChoreoRoutineStructure { override val routine = @@ -27,12 +28,8 @@ class Experimental3PieceMid( debug = false ) - override val trajectory: MutableList = - if (isRed) { - AutoUtil.transformForRed( - ChoreoTrajectory.createTrajectory("3_Piece_Mid") - ) - } else { - ChoreoTrajectory.createTrajectory("3_Piece_Mid") - } + override val trajectory: MutableList = ChoreoTrajectory.createTrajectory( + "3_Piece_Mid", + alliance + ) } diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePiece.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePiece.kt index 769f2a2..c2f46ef 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePiece.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePiece.kt @@ -1,5 +1,6 @@ package frc.team449.robot2024.auto.routines +import edu.wpi.first.wpilibj.DriverStation.Alliance import frc.team449.control.auto.ChoreoRoutine import frc.team449.control.auto.ChoreoRoutineStructure import frc.team449.control.auto.ChoreoTrajectory @@ -8,7 +9,7 @@ import frc.team449.robot2024.auto.AutoUtil class FivePiece( robot: Robot, - isRed: Boolean + alliance: Alliance ) : ChoreoRoutineStructure { override val routine = ChoreoRoutine( @@ -30,12 +31,8 @@ class FivePiece( debug = false ) - override val trajectory: MutableList = - if (isRed) { - AutoUtil.transformForRed( - ChoreoTrajectory.createTrajectory("4_Piece_Shoot_Anywhere") - ) - } else { - ChoreoTrajectory.createTrajectory("4_Piece_Shoot_Anywhere") - } + override val trajectory: MutableList = ChoreoTrajectory.createTrajectory( + "4_Piece_Shoot_Anywhere", + alliance + ) } diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePieceSubwoofer.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePieceSubwoofer.kt index fae5a25..1afae64 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePieceSubwoofer.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/FivePieceSubwoofer.kt @@ -1,5 +1,6 @@ package frc.team449.robot2024.auto.routines +import edu.wpi.first.wpilibj.DriverStation.Alliance import frc.team449.control.auto.ChoreoRoutine import frc.team449.control.auto.ChoreoRoutineStructure import frc.team449.control.auto.ChoreoTrajectory @@ -8,7 +9,7 @@ import frc.team449.robot2024.auto.AutoUtil class FivePieceSubwoofer( robot: Robot, - isRed: Boolean + alliance: Alliance ) : ChoreoRoutineStructure { override val routine = ChoreoRoutine( @@ -35,12 +36,8 @@ class FivePieceSubwoofer( timeout = 0.0 ) - override val trajectory: MutableList = - if (isRed) { - AutoUtil.transformForRed( - ChoreoTrajectory.createTrajectory("5_Piece_Sub") - ) - } else { - ChoreoTrajectory.createTrajectory("5_Piece_Sub") - } + override val trajectory: MutableList = ChoreoTrajectory.createTrajectory( + "5_Piece_Sub", + alliance + ) } diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferAmp.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferAmp.kt index c980c6e..cbbc7a2 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferAmp.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferAmp.kt @@ -1,5 +1,6 @@ package frc.team449.robot2024.auto.routines +import edu.wpi.first.wpilibj.DriverStation.Alliance import frc.team449.control.auto.ChoreoRoutine import frc.team449.control.auto.ChoreoRoutineStructure import frc.team449.control.auto.ChoreoTrajectory @@ -8,7 +9,7 @@ import frc.team449.robot2024.auto.AutoUtil class FourPieceSubwooferAmp( robot: Robot, - isRed: Boolean + alliance: Alliance ) : ChoreoRoutineStructure { override val routine = ChoreoRoutine( @@ -33,12 +34,8 @@ class FourPieceSubwooferAmp( debug = false ) - override val trajectory: MutableList = - if (isRed) { - AutoUtil.transformForRed( - ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_AmpSide") - ) - } else { - ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_AmpSide") - } + override val trajectory: MutableList = ChoreoTrajectory.createTrajectory( + "4_Piece_Subwoofer_AmpSide", + alliance + ) } diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferStage.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferStage.kt index e3fdd9c..a9669ff 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferStage.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceSubwooferStage.kt @@ -1,5 +1,6 @@ package frc.team449.robot2024.auto.routines +import edu.wpi.first.wpilibj.DriverStation.Alliance import frc.team449.control.auto.ChoreoRoutine import frc.team449.control.auto.ChoreoRoutineStructure import frc.team449.control.auto.ChoreoTrajectory @@ -8,7 +9,7 @@ import frc.team449.robot2024.auto.AutoUtil class FourPieceSubwooferStage( robot: Robot, - isRed: Boolean + alliance: Alliance ) : ChoreoRoutineStructure { override val routine = ChoreoRoutine( @@ -33,12 +34,8 @@ class FourPieceSubwooferStage( debug = false ) - override val trajectory: MutableList = - if (isRed) { - AutoUtil.transformForRed( - ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_StageSide") - ) - } else { - ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_StageSide") - } + override val trajectory: MutableList = ChoreoTrajectory.createTrajectory( + "4_Piece_Subwoofer_StageSide", + alliance + ) } diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/RoutineChooser.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/RoutineChooser.kt index 5cdee33..4884e48 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/RoutineChooser.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/RoutineChooser.kt @@ -1,5 +1,6 @@ package frc.team449.robot2024.auto.routines +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.smartdashboard.SendableChooser import edu.wpi.first.wpilibj2.command.Command import frc.team449.robot2024.Robot @@ -9,14 +10,14 @@ class RoutineChooser(private val robot: Robot) : SendableChooser() { fun routineMap(): HashMap { return hashMapOf( "DoNothing" to DoNothing(robot).createCommand(), - "Red4PieceAmp" to FourPieceSubwooferAmp(robot, true).createCommand(), - "Blue4PieceAmp" to FourPieceSubwooferAmp(robot, false).createCommand(), - "Red4PieceStage" to FourPieceSubwooferStage(robot, true).createCommand(), - "Blue4PieceStage" to FourPieceSubwooferStage(robot, false).createCommand(), - "Red3PieceMid" to Experimental3PieceMid(robot, true).createCommand(), - "Blue3PieceMid" to Experimental3PieceMid(robot, false).createCommand(), - "RedSubwoofer5Piece" to FivePieceSubwoofer(robot, true).createCommand(), - "BlueSubwoofer5Piece" to FivePieceSubwoofer(robot, false).createCommand(), + "Red4PieceAmp" to FourPieceSubwooferAmp(robot, DriverStation.Alliance.Red).createCommand(), + "Blue4PieceAmp" to FourPieceSubwooferAmp(robot, DriverStation.Alliance.Blue).createCommand(), + "Red4PieceStage" to FourPieceSubwooferStage(robot, DriverStation.Alliance.Red).createCommand(), + "Blue4PieceStage" to FourPieceSubwooferStage(robot, DriverStation.Alliance.Blue).createCommand(), + "Red3PieceMid" to Experimental3PieceMid(robot, DriverStation.Alliance.Red).createCommand(), + "Blue3PieceMid" to Experimental3PieceMid(robot, DriverStation.Alliance.Blue).createCommand(), + "RedSubwoofer5Piece" to FivePieceSubwoofer(robot, DriverStation.Alliance.Red).createCommand(), + "BlueSubwoofer5Piece" to FivePieceSubwoofer(robot, DriverStation.Alliance.Blue).createCommand(), ) }