Merge remote-tracking branch 'origin/main'

blair-robot-project · Mar 2, 2024 · 6c5faba · 6c5faba
2 parents d236eaa + cf5db22
commit 6c5faba
Show file tree

Hide file tree

Showing 7 changed files with 44 additions and 56 deletions.
diff --git 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<ChoreoTrajectory> {
       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
@@ -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<ChoreoTrajectory> =
-    if (isRed) {
-      AutoUtil.transformForRed(
-        ChoreoTrajectory.createTrajectory("3_Piece_Mid")
-      )
-    } else {
-      ChoreoTrajectory.createTrajectory("3_Piece_Mid")
-    }
+  override val trajectory: MutableList<ChoreoTrajectory> = 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
@@ -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<ChoreoTrajectory> =
-    if (isRed) {
-      AutoUtil.transformForRed(
-        ChoreoTrajectory.createTrajectory("4_Piece_Shoot_Anywhere")
-      )
-    } else {
-      ChoreoTrajectory.createTrajectory("4_Piece_Shoot_Anywhere")
-    }
+  override val trajectory: MutableList<ChoreoTrajectory> = 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
@@ -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<ChoreoTrajectory> =
-    if (isRed) {
-      AutoUtil.transformForRed(
-        ChoreoTrajectory.createTrajectory("5_Piece_Sub")
-      )
-    } else {
-      ChoreoTrajectory.createTrajectory("5_Piece_Sub")
-    }
+  override val trajectory: MutableList<ChoreoTrajectory> = 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
@@ -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<ChoreoTrajectory> =
-    if (isRed) {
-      AutoUtil.transformForRed(
-        ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_AmpSide")
-      )
-    } else {
-      ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_AmpSide")
-    }
+  override val trajectory: MutableList<ChoreoTrajectory> = 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
@@ -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<ChoreoTrajectory> =
-    if (isRed) {
-      AutoUtil.transformForRed(
-        ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_StageSide")
-      )
-    } else {
-      ChoreoTrajectory.createTrajectory("4_Piece_Subwoofer_StageSide")
-    }
+  override val trajectory: MutableList<ChoreoTrajectory> = 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
@@ -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<String>() {
   fun routineMap(): HashMap<String, Command> {
     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(),
     )
   }