pre worlds auto changes

team4099 · Apr 17, 2024 · 219dbc5 · 219dbc5
1 parent 38b5483
commit 219dbc5
Show file tree

Hide file tree

Showing 26 changed files with 661 additions and 410 deletions.
diff --git a/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt b/src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt
@@ -1,7 +1,6 @@
 package com.team4099.lib.trajectory
 
 import edu.wpi.first.math.controller.PIDController
-import edu.wpi.first.math.controller.ProfiledPIDController
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds

diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt
@@ -3,7 +3,6 @@ package com.team4099.robot2023
 import com.team4099.lib.hal.Clock
 import com.team4099.robot2023.auto.AutonomousSelector
 import com.team4099.robot2023.auto.PathStore
-import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.subsystems.falconspin.MotorChecker
@@ -24,7 +23,6 @@ import edu.wpi.first.wpilibj.simulation.DriverStationSim
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.CommandScheduler
 import edu.wpi.first.wpilibj2.command.Commands.runOnce
-import edu.wpi.first.wpilibj2.command.WaitCommand
 import org.ejml.EjmlVersion.BUILD_DATE
 import org.ejml.EjmlVersion.DIRTY
 import org.ejml.EjmlVersion.GIT_BRANCH
@@ -130,15 +128,15 @@ object Robot : LoggedRobot() {
 
     // Set the scheduler to log events for command initialize, interrupt, finish
     CommandScheduler.getInstance().onCommandInitialize { command: Command ->
-      DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", true)
+      Logger.recordOutput("/ActiveCommands/${command.name}", true)
     }
 
     CommandScheduler.getInstance().onCommandFinish { command: Command ->
-      DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false)
+      Logger.recordOutput("/ActiveCommands/${command.name}", false)
     }
 
     CommandScheduler.getInstance().onCommandInterrupt { command: Command ->
-      DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false)
+      Logger.recordOutput("/ActiveCommands/${command.name}", false)
     }
 
     val autoTab = Shuffleboard.getTab("Pre-match")
@@ -153,8 +151,8 @@ object Robot : LoggedRobot() {
   override fun autonomousInit() {
     RobotContainer.setSteeringCoastMode()
 
-    val autonCommandWithWait = runOnce({ RobotContainer.zeroSensors(isInAutonomous = true)})
-      .andThen(autonomousCommand)
+    val autonCommandWithWait =
+      runOnce({ RobotContainer.zeroSensors(isInAutonomous = true) }).andThen(autonomousCommand)
     autonCommandWithWait?.schedule()
   }
 

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -2,6 +2,7 @@ package com.team4099.robot2023
 
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.auto.AutonomousSelector
+import com.team4099.robot2023.commands.drivetrain.LockDrivetrainCommand
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand
 import com.team4099.robot2023.commands.drivetrain.TargetNoteCommand
@@ -15,7 +16,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
-import com.team4099.robot2023.subsystems.elevator.ElevatorIO
+import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.feeder.FeederIONeo
@@ -48,8 +49,6 @@ import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
-import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
-import com.team4099.robot2023.subsystems.intake.IntakeIO
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -171,42 +170,46 @@ object RobotContainer {
     limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
-    ControlBoard.intake.whileTrue(ParallelCommandGroup(
+    ControlBoard.intake.whileTrue(
+      ParallelCommandGroup(
         superstructure.groundIntakeCommand(),
-      TargetNoteCommand(
+        TargetNoteCommand(
+          driver = Ryan(),
+          { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+          { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+          { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
+          { ControlBoard.slowMode },
+          drivetrain,
+          limelight,
+          feeder
+        )
+      )
+    )
+
+    ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand())
+
+    ControlBoard.passingShotAlignment.whileTrue(
+      TargetAngleCommand(
         driver = Ryan(),
         { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
         { ControlBoard.slowMode },
-        drivetrain, limelight, feeder)
-    )
+        drivetrain,
+        {
+          if (DriverStation.getAlliance().isPresent &&
+            DriverStation.getAlliance().get() == DriverStation.Alliance.Red
+          )
+            225.degrees + 180.degrees
+          else if (DriverStation.getAlliance().isPresent &&
+            DriverStation.getAlliance().get() == DriverStation.Alliance.Blue
+          )
+            135.degrees
+          else 135.degrees
+        },
+      )
     )
 
-    ControlBoard.prepAmp.whileTrue(superstructure.prepAmpCommand())
-
-    ControlBoard.passingShotAlignment.whileTrue(TargetAngleCommand(
-      driver = Ryan(),
-      { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-      { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-      { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
-      { ControlBoard.slowMode },
-      drivetrain,
-      {
-        if (DriverStation.getAlliance().isPresent &&
-          DriverStation.getAlliance().get() == DriverStation.Alliance.Red
-        )
-        225.degrees + 180.degrees
-        else if (DriverStation.getAlliance().isPresent &&
-          DriverStation.getAlliance().get() == DriverStation.Alliance.Blue
-        )
-          135.degrees
-        else
-        135.degrees
-      },
-    ))
-
-
     ControlBoard.prepHighProtected.whileTrue(
       ParallelCommandGroup(
         superstructure.prepSpeakerMidCommand(),
@@ -237,17 +240,16 @@ object RobotContainer {
     ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
     ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand())
 
-
     ControlBoard.targetAmp.whileTrue(
-          TargetAngleCommand(
-            driver = Ryan(),
-            { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-            { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-            { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
-            { ControlBoard.slowMode },
-            drivetrain,
-            ampAngle
-          )
+      TargetAngleCommand(
+        driver = Ryan(),
+        { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+        { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+        { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
+        { ControlBoard.slowMode },
+        drivetrain,
+        ampAngle
+      )
     )
 
     ControlBoard.climbAlignFar.whileTrue(
@@ -341,6 +343,8 @@ object RobotContainer {
       )
     )
 
+    ControlBoard.lockWheels.whileTrue(LockDrivetrainCommand(drivetrain))
+
     //    ControlBoard.characterizeSubsystem.whileTrue(
     //      WheelRadiusCharacterizationCommand(
     //        drivetrain, WheelRadiusCharacterizationCommand.Companion.Direction.CLOCKWISE
@@ -377,7 +381,7 @@ object RobotContainer {
 
   fun getAutonomousLoadingCommand() = AutonomousSelector.getLoadingCommand(drivetrain)
 
-  fun resetGyroYawCommand(angle: Angle) : Command = ResetGyroYawCommand(drivetrain, angle)
+  fun resetGyroYawCommand(angle: Angle): Command = ResetGyroYawCommand(drivetrain, angle)
 
   fun mapTunableCommands() {}
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
@@ -46,6 +46,8 @@ object AutonomousSelector {
 
     autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH)
 
+    /*
+
     autonomousModeChooser.addOption(
       "Four Note Right Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH
     )
@@ -55,9 +57,14 @@ object AutonomousSelector {
     autonomousModeChooser.addOption(
       "Four Note LEFT Auto(1 Wing + 2 Centerline)", AutonomousMode.FOUR_NOTE_LEFT_AUTO_PATH
     )
+
+     */
+
     autonomousModeChooser.addOption(
       "Five Note Auto from Center Subwoofer", AutonomousMode.FIVE_NOTE_AUTO_PATH
     )
+
+    /*
     autonomousModeChooser.addOption(
       "Two Note Centerline Auto from Source Side of Subwoofer",
       AutonomousMode.TWO_NOTE_CENTERLINE_FROM_SOURCE
@@ -67,8 +74,11 @@ object AutonomousSelector {
       "Two Note Centerline Auto from Amp Side of Subwoofer",
       AutonomousMode.TWO_NOTE_CENTERLINE_FROM_AMP
     )
+
+     */
+
     autonomousModeChooser.addOption(
-      "Three Note Centerline Auto from Amp Side of Subwoofer",
+      "Three Note + Pickup Centerline Auto from Amp Side of Subwoofer",
       AutonomousMode.THREE_NOTE_CENTERLINE_FROM_AMP
     )
     autonomousModeChooser.addOption(
@@ -88,10 +98,14 @@ object AutonomousSelector {
       AutonomousMode.PRELOAD_AND_LEAVE_CENTER_SUBWOOFER
     )
 
+    /*
+
     autonomousModeChooser.addOption("Six Note Path", AutonomousMode.SIX_NOTE_AUTO_PATH)
     autonomousModeChooser.addOption(
       "Six Note Path with Pickup", AutonomousMode.SIX_NOTE_WITH_PICKUP_PATH
     )
+
+     */
     autonomousModeChooser.addOption("Test Auto Path", AutonomousMode.TEST_AUTO_PATH)
     // autonomousModeChooser.addOption("Characterize Elevator",
     // AutonomousMode.ELEVATOR_CHARACTERIZE)

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteAutoPath.kt
@@ -114,7 +114,8 @@ class FiveNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
             )
           }
         ),
-        superstructure.groundIntakeCommand()
+        superstructure
+          .groundIntakeCommand()
           .andThen(
             superstructure.prepManualSpeakerCommand(
               -3.degrees, 3000.rotations.perMinute, 0.7.degrees

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(0.5),
+      WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(
           drivetrain,
@@ -56,8 +56,9 @@ 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),
@@ -93,8 +94,9 @@ 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),
@@ -146,11 +148,41 @@ 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)
+        .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
+              ),
+              FieldWaypoint(
+                Translation2d(4.45.meters, 4.89.meters).translation2d,
+                null,
+                180.degrees.inRotation2ds
+              ),
+              FieldWaypoint(
+                Translation2d(5.92.meters, 4.0.meters).translation2d,
+                null,
+                180.degrees.inRotation2ds
+              ),
+              FieldWaypoint(
+                Translation2d(8.29.meters, 4.09.meters).translation2d,
+                null,
+                180.degrees.inRotation2ds
+              )
+            )
+          }
+        ),
+        WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
+      )
     )
   }