fix auton

FRCTeam3255 · Jul 26, 2024 · 7eac12e · 7eac12e
1 parent db02896
commit 7eac12e
Show file tree

Hide file tree

Showing 5 changed files with 7 additions and 15 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -105,7 +105,7 @@
       "useGamepad": true
     },
     {
-      "guid": "78696e70757401000000000000000000",
+      "guid": "030000004c050000e60c000000000000",
       "useGamepad": true
     },
     {

diff --git a/src/main/java/frc/robot/commands/autos/PreloadOnly.java b/src/main/java/frc/robot/commands/autos/PreloadOnly.java
@@ -75,11 +75,10 @@ public PreloadOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
     this.shoots = shoots;
 
     addCommands(
-        Commands.runOnce(
-            () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
-
         Commands.runOnce(() -> subDrivetrain.resetYaw(
             getInitialPose().get().getRotation().getDegrees())),
+        Commands.runOnce(
+            () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
 
         Commands.sequence(
             Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),
@@ -95,9 +94,6 @@ public PreloadOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
             Commands.runOnce(
                 () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
 
-            Commands.runOnce(() -> subDrivetrain.resetYaw(
-                getInitialPose().get().getRotation().getDegrees())),
-
             // PRELOAD
             // Aim
             Commands.parallel(

diff --git a/src/main/java/frc/robot/commands/autos/PreloadTaxi.java b/src/main/java/frc/robot/commands/autos/PreloadTaxi.java
@@ -66,10 +66,10 @@ public PreloadTaxi(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
     this.shoots = shoots;
 
     addCommands(
-        Commands.runOnce(
-            () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
         Commands.runOnce(() -> subDrivetrain.resetYaw(
             getInitialPose().get().getRotation().getDegrees())),
+        Commands.runOnce(
+            () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
 
         Commands.sequence(
             Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),

diff --git a/src/main/java/frc/robot/commands/autos/WingOnly.java b/src/main/java/frc/robot/commands/autos/WingOnly.java
@@ -58,10 +58,10 @@ public WingOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pitch
     this.goesDown = goesDown;
 
     addCommands(
-        Commands.runOnce(
-            () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
         Commands.runOnce(() -> subDrivetrain.resetYaw(
             getInitialPose().get().getRotation().getDegrees())),
+        Commands.runOnce(
+            () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
 
         Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),
             prefShooter.rightShooterSpeakerVelocity.getValue())),
@@ -70,8 +70,6 @@ public WingOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pitch
         Commands.runOnce(() -> subTransfer.setTransferSensorAngle(0)),
         Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false)),
 
-        new PathPlannerAuto(determinePathName() + ".1").withTimeout(0.01),
-
         // Intake until we have the game piece
         new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber),
 

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -14,14 +14,12 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;