From e00c3c05b9e6a13652f88be467bcad5535fb3c8f Mon Sep 17 00:00:00 2001 From: Anirudh S Date: Mon, 10 Jun 2024 11:44:26 -0700 Subject: [PATCH] amp auto superstructure fix --- .../team3647/frc2024/auto/AutoCommands.java | 155 ++++++++---------- .../frc2024/robot/RobotContainer.java | 2 +- .../frc2024/subsystems/Superstructure.java | 14 ++ .../frc2024/subsystems/SwerveDrive.java | 6 +- .../team3647/frc2024/util/RobotTracker.java | 2 +- src/main/java/team3647/lib/PoseUtils.java | 19 +-- 6 files changed, 100 insertions(+), 98 deletions(-) diff --git a/src/main/java/team3647/frc2024/auto/AutoCommands.java b/src/main/java/team3647/frc2024/auto/AutoCommands.java index 68f7de2..317029d 100644 --- 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 index df8ccfe..5875ff2 100644 --- 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 index 0538138..aabfa9f 100644 --- 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 index 46d1a70..cad4834 100644 --- 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 index 3df5397..b6b72e0 100644 --- 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 index 4cc2f41..870e452 100644 --- 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; } }