amp auto superstructure fix

MillenniumFalcons · Jun 10, 2024 · e00c3c0 · e00c3c0
1 parent beb2130
commit e00c3c0
Show file tree

Hide file tree

Showing 6 changed files with 100 additions and 98 deletions.
diff --git a/src/main/java/team3647/frc2024/auto/AutoCommands.java b/src/main/java/team3647/frc2024/auto/AutoCommands.java
@@ -3,7 +3,6 @@
 import com.choreo.lib.Choreo;
 import com.choreo.lib.ChoreoControlFunction;
 import com.choreo.lib.ChoreoTrajectory;
-import com.fasterxml.jackson.annotation.JsonFormat.Feature;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
@@ -22,24 +21,19 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-
 import java.util.Map;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
-
-import javax.swing.plaf.basic.BasicBorders.SplitPaneBorder;
-
 import team3647.frc2024.constants.AutoConstants;
 import team3647.frc2024.constants.FieldConstants;
 import team3647.frc2024.subsystems.Superstructure;
 import team3647.frc2024.subsystems.SwerveDrive;
 import team3647.frc2024.util.AllianceFlip;
-import team3647.frc2024.util.AutoDrive;
 import team3647.frc2024.util.AutoDrive.DriveMode;
-import team3647.lib.PoseUtils;
 import team3647.frc2024.util.TargetingUtil;
+import team3647.lib.PoseUtils;
 
 public class AutoCommands {
     private final SwerveDrive swerve;
@@ -50,8 +44,8 @@ public class AutoCommands {
     /*
      * LIST OF PATHS THAT ARE SIGNIFICANTLY MODIFIED
      * - shoot4 to f4
-     *  - 
-     * 
+     *  -
+     *
      */
     private final String s1_to_n1 = "s1 to n1";
     private final String s1_to_n1_to_f1 = "s1 to n1 to f1";
@@ -191,7 +185,6 @@ public AutoCommands(
         this.driveX = xSupplier;
         this.setAutoDriveMode = setAutoDriveMode;
 
-
         noteNotSeen = new Trigger(() -> !hasTarget.getAsBoolean()).debounce(0.5);
 
         currentYes = new Trigger(() -> superstructure.currentYes()).debounce(0.06);
@@ -259,14 +252,14 @@ public AutoCommands(
         this.redFullCenterS3 =
                 new AutonomousMode(
                         fullCenterS3(Alliance.Red), AllianceFlip.flipForPP(getInitial(s35_to_f5)));
-
-        this.redThree_S1F1AF2A = 
-                new AutonomousMode(
-                    three_S1F1AF2A(Alliance.Red), AllianceFlip.flipForPP(getInitial(s15_to_f1)));
 
-        this.blueThree_S1F1AF2A = 
+        this.redThree_S1F1AF2A =
                 new AutonomousMode(
-                    three_S1F1AF2A(Alliance.Blue), getInitial(s15_to_f1));
+                        three_S1F1AF2A(Alliance.Red),
+                        AllianceFlip.flipForPP(getInitial(s15_to_f1)));
+
+        this.blueThree_S1F1AF2A =
+                new AutonomousMode(three_S1F1AF2A(Alliance.Blue), getInitial(s15_to_f1));
     }
 
     public enum MidlineNote {
@@ -398,19 +391,17 @@ public Command six_S1F1F2N1N2N3(Alliance color) {
                         target()));
     }
 
-    public Command three_S1F1AF2A(Alliance color){
+    public Command three_S1F1AF2A(Alliance color) {
         return Commands.parallel(
-            masterSuperstructureSequence(color),
-            Commands.sequence(
-                good_followChoreoPathWithOverrideFast(s15_to_f1, color),
-                good_followChoreoPathWithOverrideFast(f1_to_amp, color),
-                Commands.waitSeconds(1),
-                good_followChoreoPathWithOverrideFast(amp_to_f2, color),
-                good_followChoreoPathWithOverrideFast(f2_to_amp, color),
-                Commands.waitSeconds(1),
-                good_followChoreoPathWithOverrideFast(amp_to_f3, color)
-            )
-        );
+                ampSuperStructureSequence(color),
+                Commands.sequence(
+                        good_followChoreoPathWithOverrideFast(s15_to_f1, color),
+                        good_followChoreoPathWithOverrideFast(f1_to_amp, color),
+                        Commands.waitSeconds(1),
+                        good_followChoreoPathWithOverrideFast(amp_to_f2, color),
+                        good_followChoreoPathWithOverrideFast(f2_to_amp, color),
+                        Commands.waitSeconds(1),
+                        good_followChoreoPathWithOverrideFast(amp_to_f3, color)));
     }
 
     public Command four_S3N5N4N3(Alliance color) {
@@ -476,8 +467,6 @@ public Command four_S1N1N2N3(Alliance color) {
                         followChoreoPathWithOverride(n2_to_n3, color)));
     }
 
-
-
     public Command pathToTrapTest() {
         return Commands.sequence(
                 pathfindToTrap(),
@@ -546,9 +535,6 @@ public Command searchOrScoreAmpToSource(BooleanSupplier hasNote, Alliance color)
                 hasNote);
     }
 
-
-
-
     public Command searchOrScoreSourceToAmp(BooleanSupplier hasNote, Alliance color) {
         return new ConditionalCommand(
                 getScoringSequenceByPoseSourceToAmp(color),
@@ -623,16 +609,13 @@ public Command masterSuperstructureSequence(Alliance color) {
                         superstructure.feed()));
     }
 
-    public Command SuperStructureSequence(Alliance color){
-        return Commands.parallel(
-            superstructure.prepAmp(),
-            continuouslyIntakeForShoot(color),
-            Commands.sequence(
-                Commands.waitUntil(() -> goodToGoAmp(color)),
-                superstructure.spinUpAmp(),
-                superstructure.spinUpAmp().alongWith(superstructure.feed())
-            )
-        );
+    public Command ampSuperStructureSequence(Alliance color) {
+        return scorePreload()
+                .andThen(
+                        Commands.parallel(
+                                superstructure.prepAmp(),
+                                intakeForAmpShot(color).repeatedly(),
+                                superstructure.feed()));
     }
 
     public boolean goodToGo(Alliance color) {
@@ -644,8 +627,8 @@ public boolean goodToGo(Alliance color) {
         }
     }
 
-    public boolean goodToGoAmp(Alliance color){
-        if(color == Alliance.Blue){
+    public boolean goodToGoAmp(Alliance color) {
+        if (color == Alliance.Blue) {
             return PoseUtils.boundingRadius(swerve.getOdoPose(), FieldConstants.kBlueAmp, 0.05);
         } else {
             return PoseUtils.boundingRadius(swerve.getOdoPose(), FieldConstants.kRedAmp, 0.05);
@@ -683,11 +666,11 @@ public Command shoot() {
         return Commands.parallel(superstructure.shootStow());
     }
 
-    public Command setPiece(){
+    public Command setPiece() {
         return Commands.runOnce(() -> this.hasPiece = true);
     }
 
-    public Command setNoPeice(){
+    public Command setNoPeice() {
         return Commands.runOnce(() -> this.hasPiece = false);
     }
 
@@ -703,7 +686,17 @@ public Command continuouslyIntakeForShoot(Alliance color) {
                                         new Trigger(() -> goodToGo(color)))));
     }
 
-
+    public Command intakeForAmpShot(Alliance color) {
+        return Commands.sequence(
+                setNoPeice(),
+                superstructure
+                        .intake()
+                        .until(currentYes)
+                        .andThen(setPiece())
+                        .andThen(
+                                superstructure.passToShooterForAmp(
+                                        new Trigger(() -> goodToGoAmp(color)))));
+    }
 
     public Pose2d flipForPP(Pose2d pose) {
         return new Pose2d(
@@ -720,7 +713,6 @@ public Command pathAndShootWithOverride(String path, Alliance color) {
                 superstructure.shootStow(), followChoreoPathWithOverride(path, color));
     }
 
-
     public Command target() {
         return Commands.run(() -> swerve.drive(0, 0, deeTheta()), swerve);
     }
@@ -775,43 +767,40 @@ public Command followChoreoPathWithOverrideFast(String path, Alliance color) {
                 .andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
     }
 
-    //also accounts for amp auto
-    public Command good_followChoreoPathWithOverrideFast(String path, Alliance color){
+    // also accounts for amp auto
+    public Command good_followChoreoPathWithOverrideFast(String path, Alliance color) {
         ChoreoTrajectory traj = Choreo.getTrajectory(path);
         boolean mirror = color == Alliance.Red;
         PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
         boolean isAmp = path.toLowerCase().endsWith("amp");
         return customChoreoFolloweForOverride(
-                        traj, 
-                        swerve::getOdoPose, 
+                        traj,
+                        swerve::getOdoPose,
                         choreoSwerveController(
-                            AutoConstants.kXController, 
-                            AutoConstants.kYController, 
-                            AutoConstants.kRotController),
-                            (ChassisSpeeds speeds) -> {
-                                var motionXComponent = speeds.vxMetersPerSecond;
-                                var motionYComponent = speeds.vyMetersPerSecond;
-                                var motionTurnComponent = isAmp ? speeds.omegaRadiansPerSecond : deeTheta();
-                                boolean nearMidline = swerve.getOdoPose().getX() > (!mirror ? 
-                                                                                4 : 
-                                                                                FieldConstants.kFieldLength - 6.9212);
-                                nearMidline &= swerve.getOdoPose().getX()
-                                                                < (!mirror
-                                                                        ? 6.9212
-                                                                        : FieldConstants
-                                                                                        .kFieldLength
-                                                                                - 4);
-
-                                if(!hasPiece && hasTarget.getAsBoolean() && nearMidline){
-                                    motionXComponent = autoDriveVelocities.get().dx;
-                                    motionYComponent = autoDriveVelocities.get().dy;
-                                }
-
-                                swerve.drive(motionXComponent, motionYComponent, motionTurnComponent);
-                            },
-                            () -> mirror)
-                            .andThen(
-                                Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
+                                AutoConstants.kXController,
+                                AutoConstants.kYController,
+                                AutoConstants.kRotController),
+                        (ChassisSpeeds speeds) -> {
+                            var motionXComponent = speeds.vxMetersPerSecond;
+                            var motionYComponent = speeds.vyMetersPerSecond;
+                            var motionTurnComponent =
+                                    isAmp ? speeds.omegaRadiansPerSecond : deeTheta();
+                            boolean nearMidline =
+                                    swerve.getOdoPose().getX()
+                                            > (!mirror ? 4 : FieldConstants.kFieldLength - 6.9212);
+                            nearMidline &=
+                                    swerve.getOdoPose().getX()
+                                            < (!mirror ? 6.9212 : FieldConstants.kFieldLength - 4);
+
+                            if (!hasPiece && hasTarget.getAsBoolean() && nearMidline) {
+                                motionXComponent = autoDriveVelocities.get().dx;
+                                motionYComponent = autoDriveVelocities.get().dy;
+                            }
+
+                            swerve.drive(motionXComponent, motionYComponent, motionTurnComponent);
+                        },
+                        () -> mirror)
+                .andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
     }
 
     public Command followChoreoPathWithOverrideFastOnTheMove(String path, Alliance color) {
@@ -950,7 +939,7 @@ public Command customChoreoFolloweForOverrideSlow(
                             trajectory
                                     .sample(timer.get(), mirrorTrajectory.getAsBoolean())
                                     .getPose());
-                    
+
                     outputChassisSpeeds.accept(
                             controller.apply(
                                     poseSupplier.get(),
@@ -1010,11 +999,11 @@ public double deeTheta() {
         return autoDriveVelocities.get().dtheta;
     }
 
-    public double deeX(){
+    public double deeX() {
         return autoDriveVelocities.get().dx;
     }
 
-    public double deeY(){
+    public double deeY() {
         return autoDriveVelocities.get().dy;
     }
 

diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java
@@ -338,7 +338,7 @@ public void configureSmartDashboardLogging() {
         // printer.addDouble("auto drive", () -> autoDrive.getVelocities().dtheta);
     }
 
-    public boolean getIsRed(){
+    public boolean getIsRed() {
         return this.runningMode.getPathplannerPose2d().getX() > 6.461565219116211;
     }
 

diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java
@@ -454,6 +454,20 @@ public Command passToShooterNoKicker(Trigger shouldGO) {
                 .andThen(Commands.deadline(shootThroughNoKicker(), spinUp()));
     }
 
+    public Command passToShooterForAmp(Trigger shouldGo) {
+        return Commands.parallel(
+                        intakeCommands.kill(),
+                        wristCommands.setAngle(() -> wrist.getInverseKinematics(pivot.getAngle())))
+                .until(
+                        shouldGo.and(
+                                () ->
+                                        wrist.angleReached(
+                                                wrist.getInverseKinematics(pivot.getAngle()), 5)))
+                .andThen(
+                        Commands.deadline(
+                                shootThroughNoKicker(), spinUpAmp(), prepAmp(), deployChurro()));
+    }
+
     public Command shootThrough() {
         return Commands.parallel(intakeCommands.intake(), kickerCommands.fastKick())
                 // pivotCommands.setAngle(() -> 20))

diff --git a/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java
@@ -263,8 +263,10 @@ public void reset() {
     public boolean underStage() {
         return ((getOdoPose().getX() > 3.2 && getOdoPose().getX() < 6.5)
                         || (getOdoPose().getX() > 9.9 && getOdoPose().getX() < 13.3))
-
-                && ((Math.abs(getOdoPose().getY() - 4)/*dist from mid */ < ((getOdoPose().getX() - 2.6) * 1 / 1.73 /*slope of stage*/)
+                && ((Math.abs(getOdoPose().getY() - 4) /*dist from mid */
+                                        < ((getOdoPose().getX() - 2.6)
+                                                * 1
+                                                / 1.73 /*slope of stage*/)
                                 && getOdoPose().getX() < 6.5)
                         || (Math.abs(getOdoPose().getY() - 4)
                                         < ((13.9 - getOdoPose().getX()) * 1 / 1.73)

diff --git a/src/main/java/team3647/frc2024/util/RobotTracker.java b/src/main/java/team3647/frc2024/util/RobotTracker.java
@@ -135,7 +135,7 @@ public double getDistanceFromSpeaker(Pose2d pose) {
         return pose.transformBy(robotToShooter).minus(speakerPose).getTranslation().getNorm();
     }
 
-    public double getDistanceFromPose(Pose2d pose){
+    public double getDistanceFromPose(Pose2d pose) {
         return periodicIO.pose.transformBy(robotToShooter).minus(pose).getTranslation().getNorm();
     }
 

diff --git a/src/main/java/team3647/lib/PoseUtils.java b/src/main/java/team3647/lib/PoseUtils.java
@@ -1,34 +1,31 @@
 package team3647.lib;
 
-import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 
 public class PoseUtils {
-    
+
     /**
-     * 
      * @param radius In meters
      * @return if the pose is within the radius given
      */
-    public static boolean boundingRadius(Pose2d measure, Pose2d target, double radius){
+    public static boolean boundingRadius(Pose2d measure, Pose2d target, double radius) {
         return Math.abs(measure.getX() - target.getX()) < radius
-            && Math.abs(measure.getY() - target.getY()) < radius;
+                && Math.abs(measure.getY() - target.getY()) < radius;
     }
 
-    public static boolean boundingTriangle(Pose2d measure, Pose2d one, Pose2d two, Pose2d three){
+    public static boolean boundingTriangle(Pose2d measure, Pose2d one, Pose2d two, Pose2d three) {
         return false;
     }
 
-    /** 
-     * |----+y+y|
-     * |        x       |
-     * |--------|      ---
+    /**
+     * |----+y+y| | x | |--------| ---
+     *
      * @param measure
      * @param x
      * @param y
      * @return
      */
-    public static boolean boundingBox(Pose2d measure, Pose2d target, double x, double y){
+    public static boolean boundingBox(Pose2d measure, Pose2d target, double x, double y) {
         return false;
     }
 }