Drive rewrite

simgui-ds.json

@@ -4,6 +4,11 @@
       "visible": true
     }
   },
+  "System Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

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,

src/main/kotlin/com/team4099/robot2023/Robot.kt

@@ -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()

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(

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,

src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt

@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inRotation2ds
@@ -23,7 +24,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,
@@ -54,11 +55,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())
+        WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
       ),
+      superstructure.prepSpeakerLowCommand(),
       superstructure
         .scoreCommand()
         .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
@@ -92,11 +91,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())
+        WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
       ),
+      superstructure.prepSpeakerLowCommand(),
       superstructure
         .scoreCommand()
         .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
@@ -146,15 +143,12 @@ 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())
+        WaitCommand(0.3).andThen(superstructure.groundIntakeCommand())
       ),
+      superstructure.prepSpeakerLowCommand(),
       superstructure
         .scoreCommand()
         .withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
-      WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
       ParallelCommandGroup(
         DrivePathCommand.createPathInFieldFrame(
           drivetrain,
@@ -164,12 +158,12 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
                 startingPose.translation.translation2d, null, 180.degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(4.45.meters, 4.89.meters).translation2d,
+                Translation2d(4.35.meters, 4.85.meters).translation2d,
                 null,
                 180.degrees.inRotation2ds
               ),
               FieldWaypoint(
-                Translation2d(5.92.meters, 4.0.meters).translation2d,
+                Translation2d(5.92.meters, 3.9.meters).translation2d,
                 null,
                 180.degrees.inRotation2ds
               ),
@@ -187,6 +181,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
   }
 
   companion object {
-    val startingPose = Pose2d(Translation2d(1.42.meters, 5.535.meters), 180.degrees)
+    val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees)
   }
 }