Worlds 2024 code (#48)

* worlds load in debugging * worlds q1-19 * code day 1 * adjust amp center line without first note * fried at worlds but its over? * tune swerve turning * Custom logger (#49) * add recordOutput method to DebugLogger * rename to CustomLogger * fix photonlib.json (now builds) * add processInputs as a method under CustomLogger --------- Co-authored-by: Matthew Choulas <[email protected]> Co-authored-by: AlphaPranav9102 <[email protected]> Co-authored-by: 00magikarp <[email protected]>
team4099 · Jun 19, 2024 · 28b7e21 · 28b7e21
1 parent d1421ed
commit 28b7e21
Show file tree

Hide file tree

Showing 44 changed files with 807 additions and 563 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/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path
@@ -112,12 +112,28 @@
     },
     {
       "anchor": {
-        "x": 8.06421609994235,
-        "y": 5.87883546728025
+        "x": 4.387298188658877,
+        "y": 4.872072048831598
       },
       "prevControl": {
-        "x": 4.787635103213693,
-        "y": 7.584219112563609
+        "x": 1.1107171919302203,
+        "y": 6.5774556941149545
+      },
+      "nextControl": {
+        "x": 7.663879185387534,
+        "y": 3.1666884035482408
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.862336685610871,
+        "y": 3.9777719789814583
+      },
+      "prevControl": {
+        "x": 7.51532475874072,
+        "y": 3.0879616157138097
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt
@@ -8,7 +8,7 @@ import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.subsystems.falconspin.MotorChecker
 import com.team4099.robot2023.util.Alert
 import com.team4099.robot2023.util.Alert.AlertType
-import com.team4099.robot2023.util.DebugLogger
+import com.team4099.robot2023.util.CustomLogger
 import com.team4099.robot2023.util.FMSData
 import com.team4099.robot2023.util.NTSafePublisher
 import edu.wpi.first.hal.AllianceStationID
@@ -197,7 +197,7 @@ object Robot : LoggedRobot() {
       "LoggedRobot/RemainingRamMB", Runtime.getRuntime().freeMemory() / 1024 / 1024
     )
 
-    DebugLogger.recordDebugOutput(
+    CustomLogger.recordDebugOutput(
       "LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds
     )
 
@@ -213,7 +213,6 @@ object Robot : LoggedRobot() {
   }
 
   override fun teleopInit() {
-    FMSData.allianceColor = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue)
     RobotContainer.zeroSensors(isInAutonomous = false)
     RobotContainer.mapTeleopControls()
     RobotContainer.getAutonomousCommand().cancel()

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -2,11 +2,8 @@ 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
-import com.team4099.robot2023.commands.drivetrain.TargetSpeakerCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
@@ -38,7 +35,7 @@ import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
 import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
 import com.team4099.robot2023.subsystems.wrist.WristIOTalon
-import com.team4099.robot2023.util.driver.Ryan
+import com.team4099.robot2023.util.driver.Jessika
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
@@ -113,16 +110,13 @@ object RobotContainer {
 
     drivetrain.defaultCommand =
       TeleopDriveCommand(
-        driver = Ryan(),
+        driver = Jessika(),
         { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
         { ControlBoard.slowMode },
         drivetrain
       )
-
-    superstructure.defaultCommand = superstructure.forceIdleIfAutoAimCommand()
-
     /*
     module steeing tuning
 
@@ -170,11 +164,15 @@ object RobotContainer {
     limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
+
+    ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand())
+
+    /*
     ControlBoard.intake.whileTrue(
       ParallelCommandGroup(
         superstructure.groundIntakeCommand(),
         TargetNoteCommand(
-          driver = Ryan(),
+          driver = Jessika(),
           { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
           { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
           { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -186,11 +184,13 @@ object RobotContainer {
       )
     )
 
+     */
+
     ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand())
 
     ControlBoard.passingShotAlignment.whileTrue(
       TargetAngleCommand(
-        driver = Ryan(),
+        driver = Jessika(),
         { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -214,7 +214,7 @@ object RobotContainer {
       ParallelCommandGroup(
         superstructure.prepSpeakerMidCommand(),
         TargetAngleCommand(
-          driver = Ryan(),
+          driver = Jessika(),
           { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
           { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
           { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -240,9 +240,10 @@ object RobotContainer {
     ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
     ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand())
 
+    /*
     ControlBoard.targetAmp.whileTrue(
       TargetAngleCommand(
-        driver = Ryan(),
+        driver = Jessika(),
         { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -252,6 +253,8 @@ object RobotContainer {
       )
     )
 
+     */
+
     ControlBoard.climbAlignFar.whileTrue(
       runOnce({
         setClimbAngle =
@@ -286,7 +289,7 @@ object RobotContainer {
     )
     ControlBoard.climbAutoAlign.whileTrue(
       TargetAngleCommand(
-        driver = Ryan(),
+        driver = Jessika(),
         { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -297,7 +300,7 @@ object RobotContainer {
     )
     //    ControlBoard.climbAlignLeft.whileTrue(
     //      TargetAngleCommand(
-    //        driver = Ryan(),
+    //        driver = Jessika(),
     //        { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
     //        { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
     //        { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -313,7 +316,7 @@ object RobotContainer {
     //
     //    ControlBoard.climbAlignRight.whileTrue(
     //      TargetAngleCommand(
-    //        driver = Ryan(),
+    //        driver = Jessika(),
     //        { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
     //        { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
     //        { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -327,10 +330,12 @@ object RobotContainer {
     //      )
     //    )
 
+    /*
+
     ControlBoard.targetSpeaker.whileTrue(
       ParallelCommandGroup(
         TargetSpeakerCommand(
-          driver = Ryan(),
+          driver = Jessika(),
           { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
           { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
           { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
@@ -343,7 +348,9 @@ 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
@@ -128,7 +142,7 @@ object AutonomousSelector {
   }
 
   val waitTime: Time
-    get() = 0.0.seconds
+    get() = waitBeforeCommandSlider.getDouble(0.0).seconds
 
   val secondaryWaitTime: Time
     get() = secondaryWaitInAuto.getDouble(0.0).seconds
@@ -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,