Skip to content

Commit

Permalink
Merge pull request #133 from Robocubs/fallback-trajectories
Browse files Browse the repository at this point in the history 
Efficiency autos
  • Loading branch information
@danjburke12
danjburke12 authored Apr 4, 2024
2 parents f1fe340 + e27479d commit b04caf5
Show file tree
Hide file tree
Showing 3 changed files with 92 additions and 98 deletions.
172 changes: 75 additions & 97 deletions src/main/java/com/team1701/robot/commands/AutonomousCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,6 +195,36 @@ 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 driveToNextPiece(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();
Expand All @@ -211,6 +241,22 @@ private Rotation2d getInitialVelocityHeading(String pathName) {
return new Rotation2d(firstState.velocityX, firstState.velocityY);
}

private Pose2d getFinalPose(String pathName) {
var trajectory = Choreo.getTrajectory(pathName);
return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getFinalPose();
}

private Command efficientlyPreWarmShootAndDrive(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(driveToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback));
}

private Command followChoreoPathAndSeekNote(String pathName) {
var trajectory = Choreo.getTrajectory(pathName);
if (trajectory == null) {
Expand Down Expand Up @@ -316,18 +362,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());
}
Expand All @@ -339,17 +379,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());
}
Expand All @@ -366,32 +398,21 @@ 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());
}

/* 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(),
efficientlyPreWarmShootAndDrive("Source54CSeek.3", "Source54CSeek.4", AutoNote.M4),
driveBackPreWarmAndShoot("Source54CSeek.5"),
followChoreoPathAndPreWarm("Source54CSeek.6"),
aimAndShoot())
.withName("Source45CSeekAuto");
Expand All @@ -403,17 +424,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());
}
Expand All @@ -425,17 +438,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());
}
Expand All @@ -446,17 +451,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", "Source543Stage.4", AutoNote.M4),
efficientlyPreWarmShootAndDrive("Source543Stage.5", "Source543Stage.6", AutoNote.M3),
driveBackPreWarmAndShoot("Source543Stage.7"))
.withName("Source453stageAuto");
return new AutonomousCommand(command, mPathBuilder.buildAndClear());
}
Expand All @@ -468,17 +465,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());
}
Expand All @@ -493,14 +482,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());
}
Expand All @@ -515,13 +498,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());
Expand Down
7 changes: 6 additions & 1 deletion src/main/java/com/team1701/robot/simulation/NoteSimulator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -71,6 +72,8 @@ public class NoteSimulator extends SubsystemBase {
private final List<NoteOnField> mNotesOnField = new ArrayList<>();
private final List<NoteInRobot> mNotesInRobot = new ArrayList<>();

private static LoggedTunableBoolean mRandomlyExcludeNotes = new LoggedTunableBoolean("RandomlyExcludeNotes", false);

public record NoteSimulatorSensors(
DigitalIO intakeEntranceSensor,
DigitalIO intakeExitSensor,
Expand Down Expand Up @@ -238,7 +241,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();
Expand Down
11 changes: 11 additions & 0 deletions src/main/java/com/team1701/robot/states/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit b04caf5

Please sign in to comment.