worlds q1-19

team4099 · Apr 18, 2024 · dc499c5 · dc499c5
1 parent edfae15
commit dc499c5
Show file tree

Hide file tree

Showing 16 changed files with 717 additions and 447 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -4,6 +4,11 @@
       "visible": true
     }
   },
+  "System Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -2,7 +2,6 @@ package com.team4099.robot2023
 
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.auto.AutonomousSelector
-import com.team4099.robot2023.commands.drivetrain.LockDriveCommand
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand
 import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand
@@ -120,9 +119,6 @@ object RobotContainer {
         { ControlBoard.slowMode },
         drivetrain
       )
-
-    superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand()
-
     /*
     module steeing tuning
 
@@ -343,8 +339,7 @@ object RobotContainer {
       )
     )
 
-
-    //ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain))
+    // ControlBoard.lockWheels.whileTrue(LockDriveCommand(drivetrain))
 
     //    ControlBoard.characterizeSubsystem.whileTrue(
     //      WheelRadiusCharacterizationCommand(

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -12,7 +12,9 @@ import com.team4099.robot2023.auto.mode.PreloadAndLeaveFromSourceSubwooferAutoPa
 import com.team4099.robot2023.auto.mode.SixNoteAutoPath
 import com.team4099.robot2023.auto.mode.TestAutoPath
 import com.team4099.robot2023.auto.mode.ThreeNoteAndPickupCenterlineSourceAutoPath
+import com.team4099.robot2023.auto.mode.ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath
 import com.team4099.robot2023.auto.mode.ThreeNoteCenterlineFromAmpAutoPath
+import com.team4099.robot2023.auto.mode.ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath
 import com.team4099.robot2023.auto.mode.TwoNoteCenterlineFromAmpAutoPath
 import com.team4099.robot2023.auto.mode.TwoNoteCenterlineFromSourceAutoPath
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
@@ -81,10 +83,22 @@ object AutonomousSelector {
       "Three Note + Pickup Centerline Auto from Amp Side of Subwoofer",
       AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP
     )
+
+    autonomousModeChooser.addOption(
+      "Three Note + Pickup Centerline WITHOUT First Note Auto from Amp Side of Subwoofer",
+      AutonomousMode.THREE_NOTE_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_AMP
+    )
+
     autonomousModeChooser.addOption(
       "Three Note + Pickup Centerline Auto from Source Side of Subwoofer",
       AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE
     )
+
+    autonomousModeChooser.addOption(
+      "Three Note + Pickup Centerline WITHOUT First Note Auto from Source Side of Subwoofer",
+      AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_SOURCE
+    )
+
     autonomousModeChooser.addOption(
       "Preload + Leave from Amp Side of Subwoofer",
       AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER
@@ -222,6 +236,17 @@ object AutonomousSelector {
             drivetrain.resetFieldFrameEstimator(flippedPose)
           })
           .andThen(ThreeNoteCenterlineFromAmpAutoPath(drivetrain, superstructure))
+      AutonomousMode.THREE_NOTE_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_AMP ->
+        return WaitCommand(waitTime.inSeconds)
+          .andThen({
+            val flippedPose =
+              AllianceFlipUtil.apply(
+                ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath.startingPose
+              )
+            drivetrain.tempZeroGyroYaw(flippedPose.rotation)
+            drivetrain.resetFieldFrameEstimator(flippedPose)
+          })
+          .andThen(ThreeNoteCenterlineWithoutFirstNoteAmpAutoPath(drivetrain, superstructure))
       AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE ->
         return WaitCommand(waitTime.inSeconds)
           .andThen({
@@ -231,6 +256,21 @@ object AutonomousSelector {
             drivetrain.resetFieldFrameEstimator(flippedPose)
           })
           .andThen(ThreeNoteAndPickupCenterlineSourceAutoPath(drivetrain, superstructure))
+      AutonomousMode.THREE_NOTE_AND_PICKUP_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_SOURCE ->
+        return WaitCommand(waitTime.inSeconds)
+          .andThen({
+            val flippedPose =
+              AllianceFlipUtil.apply(
+                ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.startingPose
+              )
+            drivetrain.tempZeroGyroYaw(flippedPose.rotation)
+            drivetrain.resetFieldFrameEstimator(flippedPose)
+          })
+          .andThen(
+            ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath(
+              drivetrain, superstructure
+            )
+          )
       AutonomousMode.PRELOAD_AND_LEAVE_LEFT_SUBWOOFER ->
         return WaitCommand(waitTime.inSeconds)
           .andThen({
@@ -288,7 +328,9 @@ object AutonomousSelector {
     TWO_NOTE_CENTERLINE_FROM_SOURCE,
     TWO_NOTE_CENTERLINE_FROM_AMP,
     THREE_NOTE_CENTERLINE_FROM_AMP,
+    THREE_NOTE_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_AMP,
     THREE_NOTE_AND_PICKUP_CENTERLINE_FROM_SOURCE,
+    THREE_NOTE_AND_PICKUP_CENTERLINE_WITHOUT_FIRST_NOTE_FROM_SOURCE,
     PRELOAD_AND_LEAVE_LEFT_SUBWOOFER,
     PRELOAD_AND_LEAVE_RIGHT_SUBWOOFER,
     PRELOAD_AND_LEAVE_CENTER_SUBWOOFER,

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
@@ -23,7 +23,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
     addCommands(
       superstructure.prepSpeakerLowCommand(),
       superstructure.scoreCommand().withTimeout(0.5),
-      WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
+      WaitCommand(0.5),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(
           drivetrain,
@@ -56,9 +56,8 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
           .withTimeout(3.235 + 0.5),
         WaitCommand(0.5)
           .andThen(superstructure.groundIntakeCommand())
-          .andThen(WaitCommand(1.75))
-          .andThen(superstructure.prepSpeakerLowCommand())
       ),
+      superstructure.prepSpeakerLowCommand(),
       superstructure
         .scoreCommand()
         .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
@@ -94,9 +93,8 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
           .withTimeout(3.235 + 0.5),
         WaitCommand(0.5)
           .andThen(superstructure.groundIntakeCommand())
-          .andThen(WaitCommand(1.75))
-          .andThen(superstructure.prepSpeakerLowCommand())
       ),
+      superstructure.prepSpeakerLowCommand(),
       superstructure
         .scoreCommand()
         .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
@@ -114,7 +112,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,
@@ -130,7 +128,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,
@@ -148,20 +146,18 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
           .withTimeout(3.235 + 0.5),
         WaitCommand(0.3)
           .andThen(superstructure.groundIntakeCommand())
-          .andThen(WaitCommand(1.75))
-          .andThen(superstructure.prepSpeakerLowCommand())
       ),
+      superstructure.prepSpeakerLowCommand(),
       superstructure
         .scoreCommand()
         .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
-      WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(
           drivetrain,
           {
             listOf(
               FieldWaypoint(
-                startingPose.translation.translation2d, null, 180.degrees.inRotation2ds
+                FourNoteAutoPath.startingPose.translation.translation2d, null, 180.degrees.inRotation2ds
               ),
               FieldWaypoint(
                 Translation2d(4.45.meters, 4.89.meters).translation2d,
@@ -190,3 +186,5 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
     val startingPose = Pose2d(Translation2d(1.42.meters, 5.535.meters), 180.degrees)
   }
 }
+
+