bob changes

team4099 · Nov 18, 2024 · 8603691 · 8603691
1 parent 96ad696
commit 8603691
Show file tree

Hide file tree

Showing 7 changed files with 351 additions and 343 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -239,7 +239,6 @@ object RobotContainer {
     ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
     ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand())
 
-    /*
     ControlBoard.targetAmp.whileTrue(
       TargetAngleCommand(
         driver = Jessika(),
@@ -252,8 +251,6 @@ object RobotContainer {
       )
     )
 
-     */
-
     ControlBoard.climbAlignFar.whileTrue(
       runOnce({
         setClimbAngle =

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -48,9 +48,18 @@ object AutonomousSelector {
     //    orientationChooser.addOption("Right", 270.degrees)
     //    autoTab.add("Starting Orientation", orientationChooser)
 
-    autonomousModeChooser.addOption("Four Note Wing Auto (Amp Side Note First, Default)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST)
-    autonomousModeChooser.addOption("Four Note Wing Auto (Center Wing Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST)
-    autonomousModeChooser.addOption("Four Note Wing Auto (Source Side Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST)
+    autonomousModeChooser.addOption(
+      "Four Note Wing Auto (Amp Side Note First, Default)",
+      AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST
+    )
+    autonomousModeChooser.addOption(
+      "Four Note Wing Auto (Center Wing Note First)",
+      AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST
+    )
+    autonomousModeChooser.addOption(
+      "Four Note Wing Auto (Source Side Note First)",
+      AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST
+    )
 
     /*
 
@@ -184,15 +193,17 @@ object AutonomousSelector {
       AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST ->
         return WaitCommand(waitTime.inSeconds)
           .andThen({
-            val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose)
+            val flippedPose =
+              AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose)
             drivetrain.tempZeroGyroYaw(flippedPose.rotation)
             drivetrain.resetFieldFrameEstimator(flippedPose)
           })
           .andThen(FourNoteAutoPathWithFirstCenterSide(drivetrain, superstructure))
       AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST ->
         return WaitCommand(waitTime.inSeconds)
           .andThen({
-            val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose)
+            val flippedPose =
+              AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose)
             drivetrain.tempZeroGyroYaw(flippedPose.rotation)
             drivetrain.resetFieldFrameEstimator(flippedPose)
           })
@@ -360,4 +371,4 @@ object AutonomousSelector {
     SIX_NOTE_AUTO_PATH,
     SIX_NOTE_WITH_PICKUP_PATH
   }
-}
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
@@ -36,8 +36,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
                 startingPose.rotation.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(2.91.meters - 0.75.meters, 6.82.meters)
-                  .translation2d,
+                Translation2d(2.91.meters - 0.75.meters, 6.82.meters).translation2d,
                 null,
                 180.degrees.inRotation2ds,
               ),
@@ -111,7 +110,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               FieldWaypoint(
                 Translation2d(
                   ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
-                          0.25.meters,
+                    0.25.meters,
                   5.55.meters
                 )
                   .translation2d,
@@ -127,7 +126,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               FieldWaypoint(
                 Translation2d(
                   ((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
-                          0.25.meters,
+                    0.25.meters,
                   5.45.meters
                 )
                   .translation2d,
@@ -183,4 +182,4 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
   companion object {
     val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees)
   }
-}
+}