Updating for competition

- autos calibrate gyro properly - adjust shooter angle
team178 · Mar 9, 2024 · c82950a · c82950a
1 parent b064743
commit c82950a
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 14 deletions.
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
@@ -30,11 +30,11 @@ public class Autos {
     public static final SendableChooser<AutoCommand> autoChooser = new SendableChooser<>();
     public static final Field2d autoField = new Field2d();
 
-    public static final void initAutos(SwerveDrive swerveDrive, Shooter shooter) {
-        autoChooser.addOption("Speaker Back", new SpeakerBack(swerveDrive, shooter));
-        autoChooser.setDefaultOption("Shoot Only", new ShootOnly(shooter));
+    public static final void initAutos(SwerveDrive swerve, Shooter shooter) {
+        autoChooser.addOption("Speaker Back", new SpeakerBack(swerve, shooter));
+        autoChooser.setDefaultOption("Shoot Only", new ShootOnly(swerve, shooter));
         // autoChooser.addOption("Shoot Only", new SpeakerOnly(shooter));
-        autoChooser.addOption("Do Nothing", new NothingAuto());
+        autoChooser.addOption("Do Nothing", new NothingAuto(swerve));
         // autoChooser.addOption("Mid Double Auto", new MidDoubleAuto(swerveDrive));
         Shuffleboard.getTab("Autos")
             .add("Auto", autoChooser)

diff --git a/src/main/java/frc/robot/commands/auto/NothingAuto.java b/src/main/java/frc/robot/commands/auto/NothingAuto.java
@@ -5,14 +5,26 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.commands.AutoCommand;
 import frc.robot.commands.Autos;
+import frc.robot.subsystems.swerve.SwerveDrive;
 
 public class NothingAuto extends AutoCommand {
-    public NothingAuto() {
-        addCommands(Commands.print("Nothing auto has finished running."));
+    private Rotation2d startingRot;
+
+    public NothingAuto(SwerveDrive swerve) {
+        this(swerve, Rotation2d.fromDegrees(180));
+    }
+
+    public NothingAuto(SwerveDrive swerve, Rotation2d startingRot) {
+        this.startingRot = startingRot;
+        addCommands(
+            swerve.runSetGyroAngle(startingRot),
+            Commands.print("Nothing auto has set the current angle to " + startingRot.getDegrees() + " degrees.")
+        );
+        addRequirements(swerve);
     }
 
     @Override
     public Pose2d getStartPose() {
-        return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(0)).getInitialPose();
+        return Autos.getPathPlannerTrajectory("Get Mid Note", startingRot).getInitialPose();
     }
 }
diff --git a/src/main/java/frc/robot/commands/auto/ShootOnly.java b/src/main/java/frc/robot/commands/auto/ShootOnly.java
@@ -5,17 +5,26 @@
 import frc.robot.commands.AutoCommand;
 import frc.robot.commands.Autos;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.swerve.SwerveDrive;
 
 public class ShootOnly extends AutoCommand {
-    public ShootOnly(Shooter shooter) {
+    private Rotation2d startingRot;
+
+    public ShootOnly(SwerveDrive swerve, Shooter shooter) {
+        this(swerve, shooter, Rotation2d.fromDegrees(180));
+    }
+
+    public ShootOnly(SwerveDrive swerve, Shooter shooter, Rotation2d startingRot) {
+        this.startingRot = startingRot;
         addCommands(
+            swerve.runSetGyroAngle(startingRot),
             Autos.runSpeakerShot(shooter)
         );
-        addRequirements(shooter);
+        addRequirements(swerve, shooter);
     }
 
     @Override
     public Pose2d getStartPose() {
-        return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(180)).getInitialPose();
+        return Autos.getPathPlannerTrajectory("Get Mid Note", startingRot).getInitialPose();
     }
 }
diff --git a/src/main/java/frc/robot/commands/auto/SpeakerBack.java b/src/main/java/frc/robot/commands/auto/SpeakerBack.java
@@ -13,14 +13,22 @@ public class SpeakerBack extends AutoCommand {
     public SpeakerBack(SwerveDrive swerve, Shooter shooter) {
         addCommands(
             Autos.runSpeakerShot(shooter),
+            swerve.runSetGyroAngle(Rotation2d.fromDegrees(180)),
             new WaitCommand(1),
             Commands.runOnce(() -> {
-                swerve.rawDriveInputs(0, -2, 0, false, false);
+                swerve.rawDriveInputs(0, 2, 0, false, false);
             }, swerve),
             new WaitCommand(0.7),
-            Commands.runOnce(() -> {
-                swerve.rawDriveInputs(0, 0, 0, false, false);
+            Commands.run(() -> {
+                swerve.rawDriveInputs(0, 0, -1.5, false, false);
+                System.out.println(Math.abs(swerve.getRotation2d().getDegrees() % 360));
             }, swerve)
+                .until(() ->
+                    (Math.abs(swerve.getRotation2d().getDegrees() % 360) < 5)
+                )
+                .andThen(() -> {
+                    swerve.rawDriveInputs(0, 0, 0, false, false);
+                }, swerve)
         );
         addRequirements(swerve, shooter);
     }

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPosition.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPosition.java
@@ -3,7 +3,7 @@
 public enum ShooterPosition {
     FLAT("FLAT", 19.5),
     AMP("AMP", 90), // prev 108.7
-    SPEAKER("SPEAKER", 62.9),
+    SPEAKER("SPEAKER", 64),
     SOURCE("SOURCE", 49);
     // amp or speaker shooting method adjusted if needed could work for trap
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -376,6 +376,12 @@ public Command runZeroGyro() {
             gyro.reset();
         });
     }
+
+    public Command runSetGyroAngle(Rotation2d rot) { // ccw+ ?
+        return runOnce(() -> {
+            gyro.setYaw(rot.getDegrees());
+        });
+    }
 
     public Command runSetSpeedFactor(double factor) {
         return runOnce(() -> {
@@ -408,6 +414,10 @@ public SwerveDriveKinematics getSwerveKinematics() {
     public Pose2d getPose() {
         return swerveOdometry.getEstimatedPosition();
     }
+
+    public Rotation2d getRotation2d() {
+        return gyro.getRotation2d();
+    }
 
     public ChassisSpeeds getRobotRelativeChassisSpeeds() {
         return SwerveConstants.kSwerveKinematics.toChassisSpeeds(getModuleStates());