autos :/

Team3256 · Nov 8, 2024 · 6bb2334 · 6bb2334
1 parent 2bf2606
commit 6bb2334
Show file tree

Hide file tree

Showing 9 changed files with 553 additions and 189 deletions.
diff --git a/src/main/deploy/choreo/Amp-Leave.traj b/src/main/deploy/choreo/Amp-Leave.traj
diff --git a/src/main/deploy/choreo/Choreo.chor b/src/main/deploy/choreo/Choreo.chor
@@ -5,126 +5,126 @@
  "variables":{
   "expressions":{},
   "poses":{
-   "center1":{
+   "center5":{
     "x":{
      "exp":"8.29 m",
      "val":8.29
     },
     "y":{
-     "exp":"7.472199440002441 m",
-     "val":7.472199440002441
+     "exp":"0.7410427927970886 m",
+     "val":0.7410427927970886
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "centerSub":{
+   "wing1":{
     "x":{
-     "exp":"1.33 m",
-     "val":1.33
+     "exp":"2.8904294967651367 m",
+     "val":2.8904294967651367
     },
     "y":{
-     "exp":"5.55 m",
-     "val":5.55
+     "exp":"7.0203728675842285 m",
+     "val":7.0203728675842285
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "center4":{
+   "center2":{
     "x":{
      "exp":"8.29 m",
      "val":8.29
     },
     "y":{
-     "exp":"2.4238319396972656 m",
-     "val":2.4238319396972656
+     "exp":"5.78941011428833 m",
+     "val":5.78941011428833
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "wing1":{
+   "wing2":{
     "x":{
      "exp":"2.8904294967651367 m",
      "val":2.8904294967651367
     },
     "y":{
-     "exp":"7.0203728675842285 m",
-     "val":7.0203728675842285
+     "exp":"5.55 m",
+     "val":5.55
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "center2":{
+   "center4":{
     "x":{
      "exp":"8.29 m",
      "val":8.29
     },
     "y":{
-     "exp":"5.78941011428833 m",
-     "val":5.78941011428833
+     "exp":"2.4238319396972656 m",
+     "val":2.4238319396972656
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "wing3":{
+   "centerSub":{
     "x":{
-     "exp":"2.721848487854004 m",
-     "val":2.721848487854004
+     "exp":"1.33 m",
+     "val":1.33
     },
     "y":{
-     "exp":"4.114828109741211 m",
-     "val":4.114828109741211
+     "exp":"5.55 m",
+     "val":5.55
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "wing2":{
+   "wing3":{
     "x":{
-     "exp":"2.8904294967651367 m",
-     "val":2.8904294967651367
+     "exp":"2.721848487854004 m",
+     "val":2.721848487854004
     },
     "y":{
-     "exp":"5.55 m",
-     "val":5.55
+     "exp":"4.114828109741211 m",
+     "val":4.114828109741211
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "center5":{
+   "center3":{
     "x":{
      "exp":"8.29 m",
      "val":8.29
     },
     "y":{
-     "exp":"0.7410427927970886 m",
-     "val":0.7410427927970886
+     "exp":"4.106620788574219 m",
+     "val":4.106620788574219
     },
     "heading":{
      "exp":"0 rad",
      "val":0.0
     }
    },
-   "center3":{
+   "center1":{
     "x":{
      "exp":"8.29 m",
      "val":8.29
     },
     "y":{
-     "exp":"4.106620788574219 m",
-     "val":4.106620788574219
+     "exp":"7.472199440002441 m",
+     "val":7.472199440002441
     },
     "heading":{
      "exp":"0 rad",

diff --git a/src/main/deploy/choreo/Source-Leave.traj b/src/main/deploy/choreo/Source-Leave.traj
diff --git a/src/main/deploy/choreo/box.traj b/src/main/deploy/choreo/box.traj
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -128,7 +128,8 @@ public class RobotContainer {
   public final MappedXboxController m_operatorController =
       new MappedXboxController(ControllerConstants.kOperatorControllerPort, "operator");
 
-  private final AutoRoutines autoRoutines = new AutoRoutines(swerve);
+  private final AutoRoutines autoRoutines =
+      new AutoRoutines(swerve, pivotShooter, intake, shooter, spindex, turret);
 
   private final AutoChooser autoChooser = new AutoChooser(swerve.autoFactory, "Auto Chooser");
   private final SwerveTelemetry swerveTelemetry =
@@ -186,6 +187,8 @@ private void configureRumble() {
 
   private void configureAutoChooser() {
     autoChooser.addAutoRoutine("Box", autoRoutines::boxAuto);
+    autoChooser.addAutoRoutine("Source Leave + Preload", autoRoutines::sourceLeave);
+    autoChooser.addAutoRoutine("Amp Leave + Preload", autoRoutines::ampLeave);
   }
 
   public void setAllianceCol(boolean red) {

diff --git a/src/main/java/frc/robot/commands/AutoRoutines.java b/src/main/java/frc/robot/commands/AutoRoutines.java
@@ -12,29 +12,97 @@
 import choreo.auto.AutoTrajectory;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.pivotshooter.PivotShooter;
+import frc.robot.subsystems.pivotshooter.PivotShooterConstants;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.spindex.Spindex;
 import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.subsystems.turret.TurretConstants;
+import frc.robot.utils.Util;
 
 public final class AutoRoutines {
   /** Example static factory for an autonomous command. */
   private final CommandSwerveDrivetrain swerve;
 
+  private final PivotShooter pivotShooter;
+  private final Intake intake;
+  private final Shooter shooter;
+  private final Spindex spindex;
+  private final Turret turret;
+
   public Command boxAuto(AutoFactory factory) {
     final AutoLoop loop = factory.newLoop("box");
 
     final AutoTrajectory box = factory.trajectory("box", loop);
 
-    loop.enabled().onTrue(Commands.runOnce(() -> swerve.resetOdometry(box)).andThen(box.cmd()));
+    loop.enabled().onTrue(swerve.resetOdometryCmd(box).andThen(box.cmd()));
+
+    return loop.cmd();
+  }
+
+  public Command sourceLeave(AutoFactory factory) {
+    final AutoLoop loop = factory.newLoop("Source Leave");
 
+    final AutoTrajectory sourceLeave = factory.trajectory("Source-Leave", loop);
+    loop.enabled()
+        .onTrue(swerve.resetOdometryCmd(sourceLeave))
+        .onTrue(AutoCommands.preload(pivotShooter, spindex, shooter, turret))
+        .debounce(7)
+        .onTrue(sourceLeave.cmd());
     return loop.cmd();
   }
 
-  public AutoRoutines(CommandSwerveDrivetrain swerve) {
+  public Command ampLeave(AutoFactory factory) {
+    final AutoLoop loop = factory.newLoop("Amp Leave");
+
+    final AutoTrajectory ampLeave = factory.trajectory("Amp-Leave", loop);
+    loop.enabled()
+        .onTrue(swerve.resetOdometryCmd(ampLeave))
+        .onTrue(AutoCommands.preload(pivotShooter, spindex, shooter, turret))
+        .debounce(7)
+        .onTrue(ampLeave.cmd());
+    return loop.cmd();
+  }
+
+  public AutoRoutines(
+      CommandSwerveDrivetrain swerve,
+      PivotShooter pivotShooter,
+      Intake intake,
+      Shooter shooter,
+      Spindex spindex,
+      Turret turret) {
     this.swerve = swerve;
+    this.pivotShooter = pivotShooter;
+    this.intake = intake;
+    this.shooter = shooter;
+    this.spindex = spindex;
+    this.turret = turret;
   }
 
   private static final class AutoCommands {
     private AutoCommands() {
       throw new UnsupportedOperationException("This is a utility class!");
     }
+
+    public static Command preload(
+        PivotShooter pivotShooter, Spindex spindex, Shooter shooter, Turret turret) {
+      return Commands.parallel(
+          pivotShooter.setPosition(PivotShooterConstants.kSubWooferPreset),
+          turret.setPosition(TurretConstants.kSubPreset),
+          shooter.setVelocity(
+              ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterFollowerSpeakerRPS),
+          Commands.waitUntil(
+                  () ->
+                      Util.epsilonEquals(
+                              shooter.getMainVelocity(), ShooterConstants.kShooterSpeakerRPS, 5)
+                          && Util.epsilonEquals(
+                              shooter.getFollowerVelocity(),
+                              ShooterConstants.kShooterFollowerSpeakerRPS,
+                              5))
+              .andThen(spindex.feedNoteToShooter()));
+    }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -79,6 +79,14 @@ public Command setVelocity(double velocity, double followerVelocity) {
         .finallyDo(shooterIO::off);
   }
 
+  public double getMainVelocity() {
+    return shooterIOAutoLogged.shooterMotorVelocity;
+  }
+
+  public double getFollowerVelocity() {
+    return shooterIOAutoLogged.shooterMotorFollowerVelocity;
+  }
+
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return m_sysIdRoutine.quasistatic(direction);
   }

diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java
@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import java.util.function.Supplier;
@@ -156,6 +157,10 @@ public void resetOdometry(AutoTrajectory traj) {
         traj.getInitialPose().get().getRotation(), m_modulePositions, traj.getInitialPose().get());
   }
 
+  public Command resetOdometryCmd(AutoTrajectory traj) {
+    return Commands.runOnce(() -> resetOdometry(traj));
+  }
+
   /*
    * Both the sysid commands are specific to one particular sysid routine, change
    * which one you're trying to characterize