Skip to content

refactor of autos #95

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
SCool62 wants to merge 6 commits into
base: main
Choose a base branch
from
Open

refactor of autos #95

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
30ab62f
Add amp4Auto and helper commands. Nonfunctional.
SCool62 Nov 15, 2024
8fad9b3
Add all the autos
SCool62 Nov 15, 2024
589b9d7
Remove the dead code
SCool62 Nov 17, 2024
330b4d4
Refactor repetitve Choreo trajectory + auto intake into util command
SCool62 Nov 22, 2024
b23aee3
Stop tracking seperate `isInitialized` field
SCool62 Nov 22, 2024
24ea422
Regenerate Choreo trajectories
SCool62 Nov 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32,758 changes: 16,716 additions & 16,042 deletions choreo.chor
View file
Open in desktop

Large diffs are not rendered by default.

1,007 changes: 717 additions & 290 deletions src/main/deploy/choreo/amp 5.1.traj
View file
Open in desktop

Large diffs are not rendered by default.

1,490 changes: 1,074 additions & 416 deletions src/main/deploy/choreo/amp 5.2.traj
View file
Open in desktop

Large diffs are not rendered by default.

818 changes: 591 additions & 227 deletions src/main/deploy/choreo/amp 5.3.traj
View file
Open in desktop

Large diffs are not rendered by default.

728 changes: 528 additions & 200 deletions src/main/deploy/choreo/amp 5.4.traj
View file
Open in desktop

Large diffs are not rendered by default.

3,938 changes: 2,838 additions & 1,100 deletions src/main/deploy/choreo/amp 5.traj
View file
Open in desktop

Large diffs are not rendered by default.

1,235 changes: 864 additions & 371 deletions src/main/deploy/choreo/line test.traj
View file
Open in desktop

Large diffs are not rendered by default.

4,835 changes: 3,384 additions & 1,451 deletions src/main/deploy/choreo/source 4 center.1.traj
View file
Open in desktop

Large diffs are not rendered by default.

2,435 changes: 1,704 additions & 731 deletions src/main/deploy/choreo/source 4 center.2.traj
View file
Open in desktop

Large diffs are not rendered by default.

2,435 changes: 1,704 additions & 731 deletions src/main/deploy/choreo/source 4 center.3.traj
View file
Open in desktop

Large diffs are not rendered by default.

9,635 changes: 6,744 additions & 2,891 deletions src/main/deploy/choreo/source 4 center.traj
View file
Open in desktop

Large diffs are not rendered by default.

4,835 changes: 3,384 additions & 1,451 deletions src/main/deploy/choreo/source 4.1.traj
View file
Open in desktop

Large diffs are not rendered by default.

4,835 changes: 3,384 additions & 1,451 deletions src/main/deploy/choreo/source 4.2.traj
View file
Open in desktop

Large diffs are not rendered by default.

2,435 changes: 1,704 additions & 731 deletions src/main/deploy/choreo/source 4.3.traj
View file
Open in desktop

Large diffs are not rendered by default.

12,035 changes: 8,424 additions & 3,611 deletions src/main/deploy/choreo/source 4.traj
View file
Open in desktop

Large diffs are not rendered by default.

3,836 changes: 1,918 additions & 1,918 deletions src/main/deploy/choreo/test.1.traj
View file
Open in desktop

Large diffs are not rendered by default.

3,836 changes: 1,918 additions & 1,918 deletions src/main/deploy/choreo/test.traj
View file
Open in desktop

Large diffs are not rendered by default.

278 changes: 10 additions & 268 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@

package frc.robot;

import com.choreo.lib.Choreo;
import com.choreo.lib.ChoreoTrajectory;
import com.ctre.phoenix6.SignalLogger;
import edu.wpi.first.hal.simulation.RoboRioDataJNI;
Expand All @@ -18,7 +17,6 @@
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.Timer;
Expand All @@ -29,6 +27,7 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.autonomous.Autos;
import frc.robot.subsystems.carriage.CarriageIOReal;
import frc.robot.subsystems.carriage.CarriageSubsystem;
import frc.robot.subsystems.elevator.ElevatorIOReal;
Expand Down Expand Up @@ -188,6 +187,9 @@ public void robotInit() {
// be added.
SignalLogger.setPath("/media/sda1/");

// Initialize the Autos singleton
Autos.init(intake, shooter, feeder, swerve, carriage, this::staticAutoAim);

// Default Commands here
swerve.setDefaultCommand(
swerve.runVoltageTeleopFieldRelative(
Expand Down Expand Up @@ -452,12 +454,12 @@ public void robotInit() {

autoChooser.addOption("None", Commands.none());
autoChooser.addOption("Shoot Preload", teleopAutoAim());
autoChooser.addDefaultOption("Amp 4 Wing", autoAmp4Wing());
autoChooser.addOption("Source 3", autoSource3());
autoChooser.addOption("Center 4", autoCenter4());
autoChooser.addOption("Amp 5", autoAmp5());
autoChooser.addOption("Source 4", autoSource4());
autoChooser.addOption("Line Test Repeatedly", lineTest());
autoChooser.addDefaultOption("Amp 4 Wing", Autos.getInstance().amp4());
autoChooser.addOption("Source 3", Autos.getInstance().source3());
autoChooser.addOption("Center 4", Autos.getInstance().center4());
autoChooser.addOption("Amp 5", Autos.getInstance().amp5());
autoChooser.addOption("Source 4", Autos.getInstance().source4());
autoChooser.addOption("Line Test Repeatedly", Autos.getInstance().lineTest());

// Dashboard command buttons
SmartDashboard.putData("Shooter shoot", shootWithDashboard());
Expand Down Expand Up @@ -697,266 +699,6 @@ private Command staticAutoAim(double tolerance) {
return staticAutoAim(() -> tolerance);
}

private Command autoStaticAutoAim() {
return feeder
.indexCmd()
.deadlineWith(
swerve.runVelocityCmd(() -> new ChassisSpeeds()),
Commands.repeatingSequence(
shooter
.runFlywheelsCmd(() -> 0.0, () -> 0.0)
.unless(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0)
.until(() -> feeder.getFirstBeambreak() && swerve.getDistanceToSpeaker() < 8.0),
shooter.runStateCmd(
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS())))
.until(() -> feeder.getFirstBeambreak())
.unless(() -> feeder.getFirstBeambreak())
.withTimeout(1.0)
.andThen(
staticAutoAim(6.0)
.deadlineWith(
intake.runVoltageCmd(0, 0).asProxy(), carriage.runVoltageCmd(0).asProxy())
.withTimeout(2.0)
.unless(() -> !feeder.getFirstBeambreak()))
.asProxy()
.withTimeout(4.0);
}

private Command autoFenderShot() {
return shooter
.runStateCmd(
AutoAim.FENDER_SHOT.getRotation(),
AutoAim.FENDER_SHOT.getLeftRPS(),
AutoAim.FENDER_SHOT.getRightRPS())
.raceWith(
feeder
.runVelocityCmd(0.0)
.until(() -> shooter.isAtGoal())
.andThen(
feeder
.runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY)
.raceWith(
Commands.waitUntil(() -> !feeder.getFirstBeambreak())
.andThen(Commands.waitSeconds(0.1)))))
.asProxy();
}

private Command autoIntake() {
return Commands.parallel(
intake
.runVelocityCmd(50.0, 30.0)
.until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
.asProxy(),
feeder.indexCmd().asProxy(),
carriage
.runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE)
.until(() -> feeder.getFirstBeambreak())
.asProxy(),
shooter
.runStateCmd(
() ->
Rotation2d.fromDegrees(
MathUtil.clamp(
AutoAim.shotMap
.get(swerve.getDistanceToSpeaker())
.getRotation()
.getDegrees(),
0,
20)),
() -> 0.0,
() -> 0.0)
.until(() -> feeder.getFirstBeambreak())
.unless(() -> feeder.getFirstBeambreak())
.andThen(
shooter.runStateCmd(
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRotation(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getLeftRPS(),
() -> AutoAim.shotMap.get(swerve.getDistanceToSpeaker()).getRightRPS()))
.asProxy());
}

private Command autoAmp4Wing() {
return Commands.sequence(
autoFenderShot(),
swerve
.runChoreoTraj(Choreo.getTrajectory("amp 4 local.1"), true)
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
.withTimeout(1.0),
autoStaticAutoAim()
.andThen(Commands.print("Done with auto static auto aim"))
.beforeStarting(Commands.print("Before auto static auto aim!!!")),
swerve
.runChoreoTraj(Choreo.getTrajectory("amp 4 local.2"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
.withTimeout(1.0),
autoStaticAutoAim(),
swerve
.runChoreoTraj(Choreo.getTrajectory("amp 4 local.3"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake().until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak()),
autoStaticAutoAim());
}

private Command autoAmp5() {
return Commands.sequence(
autoFenderShot(),
swerve
.runChoreoTraj(Choreo.getTrajectory("amp 5.1"), true)
.asProxy()
.deadlineWith(autoIntake()),
autoStaticAutoAim(),
swerve
.runChoreoTraj(Choreo.getTrajectory("amp 5.2"))
.asProxy()
.deadlineWith(Commands.waitSeconds(1.0).andThen(autoIntake())),
autoStaticAutoAim(),
swerve.runChoreoTraj(Choreo.getTrajectory("amp 5.3")).asProxy().deadlineWith(autoIntake()),
autoIntake()
.until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
.withTimeout(1.0),
autoStaticAutoAim(),
swerve.runChoreoTraj(Choreo.getTrajectory("amp 5.4")).asProxy().deadlineWith(autoIntake()),
autoIntake()
.until(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
.withTimeout(1.0),
autoStaticAutoAim());
}

private Command autoCenter4() {
return Commands.sequence(
autoFenderShot(),
swerve
.runChoreoTraj(Choreo.getTrajectory("center 4.1"), true)
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()),
swerve
.runChoreoTraj(Choreo.getTrajectory("center 4.2"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()),
swerve
.runChoreoTraj(Choreo.getTrajectory("center 4.3"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim());
}

private Command autoSource3() {
return Commands.sequence(
autoFenderShot(),
swerve
.runChoreoTraj(Choreo.getTrajectory("source 3.1"), true)
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()),
swerve
.runChoreoTraj(Choreo.getTrajectory("source 3.2"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim());
}

private Command autoSource4() {
return Commands.sequence(
autoFenderShot(),
swerve
.runChoreoTraj(Choreo.getTrajectory("source 4.1"), true)
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()),
swerve
.runChoreoTraj(Choreo.getTrajectory("source 4.2"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim().unless(() -> !feeder.getFirstBeambreak()),
swerve
.runChoreoTraj(Choreo.getTrajectory("source 4.3"))
.asProxy()
.deadlineWith(autoIntake()),
autoIntake()
.raceWith(
Commands.sequence(
Commands.waitSeconds(0.25),
Commands.waitUntil(
() -> carriage.getBeambreak() || feeder.getFirstBeambreak())))
.withTimeout(1.0),
autoStaticAutoAim());
}

private Command lineTest() {
ChoreoTrajectory traj = Choreo.getTrajectory("line test");
return Commands.sequence(
Commands.runOnce(
() -> {
if (DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get().equals(Alliance.Red)) {
swerve.setPose(traj.flipped().getInitialPose());
} else {
swerve.setPose(traj.getInitialPose());
}
}),
swerve.runChoreoTraj(traj).repeatedly());
}

private Command ampHeadingSnap(DoubleSupplier x, DoubleSupplier y) {
var headingController =
new ProfiledPIDController(
Expand Down
Loading