From 3cab403b9d4a9232aa0ecbd4acca7819fd98475a Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Wed, 3 Apr 2024 10:26:30 -0400 Subject: [PATCH 1/3] Piece efficiency implemented --- .../robot/commands/AutonomousCommands.java | 64 ++++++++++++++++--- .../team1701/robot/commands/PauseDrive.java | 3 +- .../robot/simulation/NoteSimulator.java | 7 +- .../com/team1701/robot/states/RobotState.java | 11 ++++ 4 files changed, 74 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java index 565b90e9..8c77cfdc 100644 --- a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java @@ -192,11 +192,64 @@ private Command followChoreoPath(String pathName, boolean resetPose) { .withName("FollowChoreo"); } + private Command driveBackPreWarmAndShoot(String pathName) { + return loggedSequence( + pauseDrive(pathName), + followChoreoPathAndPreWarm(pathName), + aimAndShoot(), + runOnce(() -> mRobotState.setUseAutonFallback(false))); + } + + private Command followLogicalTrajectory(String choreoPathName, AutoNote nextNote) { + return either( + driveTrajectoryToNextPiece(nextNote), + driveBackPreWarmAndShoot(choreoPathName), + mRobotState::getUseAutonFallback); + } + + private Command driveTrajectoryToNextPiece(AutoNote nextNote) { + return new DriveAndSeekNote( + mDrive, + mRobotState, + new DriveToPose( + mDrive, + mRobotState, + () -> new Pose2d( + nextNote.pose().getTranslation(), + mRobotState + .getPose2d() + .getTranslation() + .minus(nextNote.pose().getTranslation()) + .getAngle()), + mRobotState::getPose2d, + kAutoTrapezoidalKinematicLimits, + true, + true), + mAutoNoteSeeker::getDetectedNoteToSeek, + kAutoTrapezoidalKinematicLimits); + } + private Pose2d getFirstPose(String pathName) { var trajectory = Choreo.getTrajectory(pathName); return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getInitialPose(); } + private Pose2d getFinalPose(String pathName) { + var trajectory = Choreo.getTrajectory(pathName); + return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getFinalPose(); + } + + private Command followPreWarmShootOrSkipToNext(String pathName, String returnPath, AutoNote nextNote) { + return race( + driveBackPreWarmAndShoot(pathName).andThen(followChoreoPathAndSeekNote(returnPath)), + waitSeconds(.7) + .andThen(either( + idle(), + runOnce(() -> mRobotState.setUseAutonFallback(true)), + mRobotState::hasNote))) + .andThen(either(driveTrajectoryToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback)); + } + private Command followChoreoPathAndSeekNote(String pathName) { var trajectory = Choreo.getTrajectory(pathName); if (trajectory == null) { @@ -359,21 +412,14 @@ public AutonomousCommand greedyMiddle() { return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } - /* Phase 2 Autons */ - public AutonomousCommand source54CSeek() { var command = loggedSequence( print("Started source 54C seek auto"), driveToPoseWhileShooting( getFirstPose("Source54CSeek.2"), FinishedState.END_AFTER_SHOOTING_AND_MOVING), followChoreoPathAndSeekNote("Source54CSeek.2"), - pauseDrive("Source54CSeek.3"), - followChoreoPathAndPreWarm("Source54CSeek.3"), - aimAndShoot(), - followChoreoPathAndSeekNote("Source54CSeek.4"), - pauseDrive("Source54CSeek.5"), - followChoreoPathAndPreWarm("Source54CSeek.5"), - aimAndShoot(), + followPreWarmShootOrSkipToNext("Source54CSeek.3", "Source54CSeek.4", AutoNote.M4), + driveBackPreWarmAndShoot("Source54CSeek.5"), followChoreoPathAndPreWarm("Source54CSeek.6"), aimAndShoot()) .withName("Source45CSeekAuto"); diff --git a/src/main/java/com/team1701/robot/commands/PauseDrive.java b/src/main/java/com/team1701/robot/commands/PauseDrive.java index 1f720504..fccde564 100644 --- a/src/main/java/com/team1701/robot/commands/PauseDrive.java +++ b/src/main/java/com/team1701/robot/commands/PauseDrive.java @@ -50,7 +50,8 @@ public void execute() { .get() .getTranslation() .minus(mRobotState.getPose2d().getTranslation()) - .getAngle()); + .getAngle() + .rotateBy(mRobotState.getHeading())); } else { mRotationCommand.execute(); } diff --git a/src/main/java/com/team1701/robot/simulation/NoteSimulator.java b/src/main/java/com/team1701/robot/simulation/NoteSimulator.java index 16e49aae..1b2e50d3 100644 --- a/src/main/java/com/team1701/robot/simulation/NoteSimulator.java +++ b/src/main/java/com/team1701/robot/simulation/NoteSimulator.java @@ -12,6 +12,7 @@ import com.team1701.lib.drivers.motors.MotorIO; import com.team1701.lib.util.GeometryUtil; import com.team1701.lib.util.Util; +import com.team1701.lib.util.tuning.LoggedTunableBoolean; import com.team1701.robot.Constants; import com.team1701.robot.FieldConstants; import com.team1701.robot.autonomous.AutoNote; @@ -71,6 +72,8 @@ public class NoteSimulator extends SubsystemBase { private final List mNotesOnField = new ArrayList<>(); private final List mNotesInRobot = new ArrayList<>(); + private static LoggedTunableBoolean mRandomlyExcludeNotes = new LoggedTunableBoolean("RandomlyExcludeNotes", false); + public record NoteSimulatorSensors( DigitalIO intakeEntranceSensor, DigitalIO intakeExitSensor, @@ -237,7 +240,9 @@ public void simulationPeriodic() { public void placeAutonNotes() { mNotesOnField.clear(); for (var note : kStartingNotePoses) { - mNotesOnField.add(new NoteOnField(note)); + if (!mRandomlyExcludeNotes.get() || Math.random() > .2) { + mNotesOnField.add(new NoteOnField(note)); + } } mNotesInRobot.clear(); diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java index 7fafc548..6307d17a 100644 --- a/src/main/java/com/team1701/robot/states/RobotState.java +++ b/src/main/java/com/team1701/robot/states/RobotState.java @@ -58,6 +58,8 @@ public class RobotState { private ShootingState mShootingState = ShootingState.kDefault; + private boolean mUseAutonFallback = false; + public RobotState() { mField = new Field2d(); SmartDashboard.putData("Field", mField); @@ -389,6 +391,15 @@ public boolean isClimbMode() { return mScoringMode == ScoringMode.CLIMB; } + public boolean getUseAutonFallback() { + return mUseAutonFallback; + } + + public boolean setUseAutonFallback(boolean use) { + mUseAutonFallback = use; + return mUseAutonFallback; + } + public enum ScoringMode { SPEAKER, AMP, From 98030c70fc85a09063925493d66faa3727b6ac9b Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Wed, 3 Apr 2024 14:10:11 -0400 Subject: [PATCH 2/3] added efficiency to autons --- .../robot/commands/AutonomousCommands.java | 130 +++++------------- 1 file changed, 31 insertions(+), 99 deletions(-) diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java index 8c77cfdc..0a92570f 100644 --- a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java @@ -200,14 +200,7 @@ private Command driveBackPreWarmAndShoot(String pathName) { runOnce(() -> mRobotState.setUseAutonFallback(false))); } - private Command followLogicalTrajectory(String choreoPathName, AutoNote nextNote) { - return either( - driveTrajectoryToNextPiece(nextNote), - driveBackPreWarmAndShoot(choreoPathName), - mRobotState::getUseAutonFallback); - } - - private Command driveTrajectoryToNextPiece(AutoNote nextNote) { + private Command driveToNextPiece(AutoNote nextNote) { return new DriveAndSeekNote( mDrive, mRobotState, @@ -239,7 +232,7 @@ private Pose2d getFinalPose(String pathName) { return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getFinalPose(); } - private Command followPreWarmShootOrSkipToNext(String pathName, String returnPath, AutoNote nextNote) { + private Command efficientlyPreWarmShootAndDrive(String pathName, String returnPath, AutoNote nextNote) { return race( driveBackPreWarmAndShoot(pathName).andThen(followChoreoPathAndSeekNote(returnPath)), waitSeconds(.7) @@ -247,7 +240,7 @@ private Command followPreWarmShootOrSkipToNext(String pathName, String returnPat idle(), runOnce(() -> mRobotState.setUseAutonFallback(true)), mRobotState::hasNote))) - .andThen(either(driveTrajectoryToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback)); + .andThen(either(driveToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback)); } private Command followChoreoPathAndSeekNote(String pathName) { @@ -351,18 +344,12 @@ public AutonomousCommand source4321CenterStage() { print("Started source 4321 center stage auto"), aimAndShoot(), followChoreoPathAndSeekNote("Source4321CenterStage.1"), - pauseDrive("Source4321CenterStage.2"), - followChoreoPathAndPreWarm("Source4321CenterStage.2"), - aimAndShoot(), - followChoreoPathAndSeekNote("Source4321CenterStage.3"), - pauseDrive("Source4321CenterStage.4"), - followChoreoPathAndPreWarm("Source4321CenterStage.4"), - aimAndShoot(), - followChoreoPathAndSeekNote("Source4321CenterStage.5"), - pauseDrive("Source4321CenterStage.6"), - followChoreoPathAndPreWarm("Source4321CenterStage.6"), - aimAndShoot(), - followChoreoPathAndSeekNote("Source4321CenterStage.7")) + efficientlyPreWarmShootAndDrive( + "Source4321CenterStage.2", "Source4321CenterStage.3", AutoNote.M3), + efficientlyPreWarmShootAndDrive( + "Source4321CenterStage.4", "Source4321CenterStage.5", AutoNote.M2), + efficientlyPreWarmShootAndDrive( + "Source4321CenterStage.6", "Source4321CenterStage.7", AutoNote.M1)) .withName("Source 4321 CenterStage Auto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -374,17 +361,9 @@ public AutonomousCommand ampA123amp() { followChoreoPathAndPreWarm("AmpA123Amp.1"), aimAndShoot(), followChoreoPathAndSeekNote("AmpA123Amp.2"), - pauseDrive("AmpA123Amp.3"), - followChoreoPathAndPreWarm("AmpA123Amp.3"), - aimAndShoot(), - followChoreoPathAndSeekNote("AmpA123Amp.4"), - pauseDrive("AmpA123Amp.5"), - followChoreoPathAndPreWarm("AmpA123Amp.5"), - aimAndShoot(), - followChoreoPathAndSeekNote("AmpA123Amp.6"), - pauseDrive("AmpA123Amp.7"), - followChoreoPathAndPreWarm("AmpA123Amp.7"), - aimAndShoot()) + efficientlyPreWarmShootAndDrive("AmpA123Amp.3", "AmpA123Amp.4", AutoNote.M2), + efficientlyPreWarmShootAndDrive("AmpA123Amp.5", "AmpA123Amp.6", AutoNote.M3), + driveBackPreWarmAndShoot("AmpA123Amp.7")) .withName("Amp A123 Amp Auto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -401,12 +380,8 @@ public AutonomousCommand greedyMiddle() { followChoreoPathAndPreWarm("GreedyMiddle.4"), aimAndShoot(), followChoreoPathAndSeekNote("GreedyMiddle.5"), - followChoreoPathAndPreWarm("GreedyMiddle.6"), - aimAndShoot(), - followChoreoPathAndSeekNote("GreedyMiddle.7"), - followChoreoPathAndPreWarm("GreedyMiddle.8"), - aimAndShoot(), - followChoreoPathAndSeekNote("GreedyMiddle.9")) + efficientlyPreWarmShootAndDrive("GreedyMiddle.6", "GreedyMiddle.7", AutoNote.M2), + efficientlyPreWarmShootAndDrive("GreedyMiddle.8", "GreedyMiddle.9", AutoNote.M3)) .withName("GreedyMiddleAuto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); @@ -418,7 +393,7 @@ public AutonomousCommand source54CSeek() { driveToPoseWhileShooting( getFirstPose("Source54CSeek.2"), FinishedState.END_AFTER_SHOOTING_AND_MOVING), followChoreoPathAndSeekNote("Source54CSeek.2"), - followPreWarmShootOrSkipToNext("Source54CSeek.3", "Source54CSeek.4", AutoNote.M4), + efficientlyPreWarmShootAndDrive("Source54CSeek.3", "Source54CSeek.4", AutoNote.M4), driveBackPreWarmAndShoot("Source54CSeek.5"), followChoreoPathAndPreWarm("Source54CSeek.6"), aimAndShoot()) @@ -431,17 +406,9 @@ public AutonomousCommand amp123Amp() { print("Started Amp123Seek auto"), aimAndShoot(), followChoreoPathAndSeekNote("Amp123Amp.1"), - pauseDrive("Amp123Amp.2"), - followChoreoPathAndPreWarm("Amp123Amp.2"), - aimAndShoot(), - followChoreoPathAndSeekNote("Amp123Amp.3"), - pauseDrive("Amp123Amp.4"), - followChoreoPathAndPreWarm("Amp123Amp.4"), - aimAndShoot(), - followChoreoPathAndSeekNote("Amp123Amp.5"), - pauseDrive("Amp123Amp.6"), - followChoreoPathAndPreWarm("Amp123Amp.6"), - aimAndShoot()) + efficientlyPreWarmShootAndDrive("Amp123Amp.2", "Amp123Amp.3", AutoNote.M2), + efficientlyPreWarmShootAndDrive("Amp123Amp.4", "Amp123Amp.5", AutoNote.M3), + driveBackPreWarmAndShoot("Amp123Amp.6")) .withName("Amp 123 Amp Auto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -453,17 +420,9 @@ public AutonomousCommand centerB342Stage() { followChoreoPathAndPreWarm("CenterB342Stage.1"), aimAndShoot(), followChoreoPathAndSeekNote("CenterB342Stage.2"), - pauseDrive("CenterB342Stage.3"), - followChoreoPathAndPreWarm("CenterB342Stage.3"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterB342Stage.4"), - pauseDrive("CenterB342Stage.5"), - followChoreoPathAndPreWarm("CenterB342Stage.5"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterB342Stage.6"), - pauseDrive("CenterB342Stage.7"), - followChoreoPathAndPreWarm("CenterB342Stage.7"), - aimAndShoot()) + efficientlyPreWarmShootAndDrive("CenterB342Stage.3", "CenterB342Stage.4", AutoNote.M4), + efficientlyPreWarmShootAndDrive("CenterB342Stage.5", "CenterB342Stage.6", AutoNote.M2), + driveBackPreWarmAndShoot("CenterB342Stage.7")) .withName("Center B342 Stage Auto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -474,17 +433,9 @@ public AutonomousCommand source543Stage() { driveToPoseWhileShooting( getFirstPose("Source543Stage.2"), FinishedState.END_AFTER_SHOOTING_AND_MOVING), followChoreoPathAndSeekNote("Source543Stage.2"), - pauseDrive("Source543Stage.3"), - followChoreoPathAndPreWarm("Source543Stage.3"), - aimAndShoot(), - followChoreoPathAndSeekNote("Source543Stage.4"), - pauseDrive("Source543Stage.5"), - followChoreoPathAndPreWarm("Source543Stage.5"), - aimAndShoot(), - followChoreoPathAndSeekNote("Source543Stage.6"), - pauseDrive("Source543Stage.6"), - followChoreoPathAndPreWarm("Source543Stage.7"), - aimAndShoot()) + efficientlyPreWarmShootAndDrive("Source543Stage.3", "CenterB342Stage.4", AutoNote.M4), + efficientlyPreWarmShootAndDrive("Source543Stage.5", "CenterB342Stage.6", AutoNote.M4), + driveBackPreWarmAndShoot("Source543Stage.7")) .withName("Source453stageAuto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -496,17 +447,9 @@ public AutonomousCommand centerB231Center() { followChoreoPathAndPreWarm("CenterB231Center.1", false, false), aimAndShoot(), followChoreoPathAndSeekNote("CenterB231Center.2"), - pauseDrive("CenterB231Center.3"), - followChoreoPathAndPreWarm("CenterB231Center.3"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterB231Center.4"), - pauseDrive("CenterB231Center.5"), - followChoreoPathAndPreWarm("CenterB231Center.5"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterB231Center.6"), - pauseDrive("CenterB231Center.7"), - followChoreoPathAndPreWarm("CenterB231Center.7"), - aimAndShoot()) + efficientlyPreWarmShootAndDrive("CenterB231Center.3", "CenterB231Center.4", AutoNote.M3), + efficientlyPreWarmShootAndDrive("CenterB231Center.5", "CenterB231Center.6", AutoNote.M1), + driveBackPreWarmAndShoot("CenterB231Center.7")) .withName("Center B231 Stage Auto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -521,14 +464,8 @@ public AutonomousCommand centerBA123Amp() { followChoreoPathAndPreWarm("CenterBA123Amp.3"), aimAndShoot(), followChoreoPathAndSeekNote("CenterBA123Amp.4"), - pauseDrive("CenterBA123Amp.5"), - followChoreoPathAndPreWarm("CenterBA123Amp.5"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterBA123Amp.6"), - pauseDrive("CenterBA123Amp.7"), - followChoreoPathAndPreWarm("CenterBA123Amp.7"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterBA123Amp.8")) + efficientlyPreWarmShootAndDrive("CenterBA123Amp.5", "CenterBA123Amp.6", AutoNote.M2), + efficientlyPreWarmShootAndDrive("CenterBA123Amp.7", "CenterBA123Amp.8", AutoNote.M3)) .withName("Center BA123 Amp Auto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); } @@ -543,13 +480,8 @@ public AutonomousCommand centerBC123center() { followChoreoPathAndPreWarm("CenterBC123Center.3"), aimAndShoot(), followChoreoPathAndSeekNote("CenterBC123Center.4"), - followChoreoPathAndPreWarm("CenterBC123Center.5"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterBC123Center.6"), - pauseDrive("CenterBC123Center.7"), - followChoreoPathAndPreWarm("CenterBC123Center.7"), - aimAndShoot(), - followChoreoPathAndSeekNote("CenterBC123Center.8")) + efficientlyPreWarmShootAndDrive("CenterBC123Center.5", "CenterBC123Center.6", AutoNote.M2), + efficientlyPreWarmShootAndDrive("CenterBC123Center.7", "CenterBC123Center.8", AutoNote.M3)) .withName("CenterBC123CenterAuto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear()); From d4f4ddb0a05f574bd3194f950ce089754dfd12da Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Wed, 3 Apr 2024 21:31:49 -0400 Subject: [PATCH 3/3] Fixed Source 543 Stage path bug --- .../java/com/team1701/robot/commands/AutonomousCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java index 0a92570f..0ece9e6f 100644 --- a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java @@ -433,8 +433,8 @@ public AutonomousCommand source543Stage() { driveToPoseWhileShooting( getFirstPose("Source543Stage.2"), FinishedState.END_AFTER_SHOOTING_AND_MOVING), followChoreoPathAndSeekNote("Source543Stage.2"), - efficientlyPreWarmShootAndDrive("Source543Stage.3", "CenterB342Stage.4", AutoNote.M4), - efficientlyPreWarmShootAndDrive("Source543Stage.5", "CenterB342Stage.6", AutoNote.M4), + efficientlyPreWarmShootAndDrive("Source543Stage.3", "Source543Stage.4", AutoNote.M4), + efficientlyPreWarmShootAndDrive("Source543Stage.5", "Source543Stage.6", AutoNote.M3), driveBackPreWarmAndShoot("Source543Stage.7")) .withName("Source453stageAuto"); return new AutonomousCommand(command, mPathBuilder.buildAndClear());