Skip to content

Commit

Permalink
Davis Auto 5 Note
Browse files Browse the repository at this point in the history
  • Loading branch information
@manthan-acharya
manthan-acharya committed Feb 14, 2024
1 parent 2c2a1a4 commit f2ffc27
Show file tree
Hide file tree
Showing 6 changed files with 208 additions and 25 deletions.
20 changes: 20 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,26 @@ public static final class Subwoofer {
Rotation2d.fromDegrees(180));
}

public static final class Stage {
public static Pose2d podiumLeg =
new Pose2d(Units.inchesToMeters(126.75), Units.inchesToMeters(161.638), new Rotation2d());

public static Pose2d sourceLeg =
new Pose2d(
Units.inchesToMeters(220.873),
Units.inchesToMeters(212.425),
Rotation2d.fromDegrees(-30));

public static Pose2d ampLeg =
new Pose2d(
Units.inchesToMeters(220.873),
Units.inchesToMeters(110.837),
Rotation2d.fromDegrees(30));

public static Pose2d center =
new Pose2d(Units.inchesToMeters(192.55), Units.inchesToMeters(161.638), new Rotation2d());
}

public static double aprilTagWidth = Units.inchesToMeters(6.50);
public static AprilTagFieldLayout aprilTags;

Expand Down
5 changes: 4 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,7 @@ public RobotContainer() {

private void configureAutos() {
autoChooser.addDefaultOption("Do Nothing", Commands.none());
AutoCommands autoCommands = new AutoCommands(drive, superstructure);
AutoCommands autoCommands = new AutoCommands(drive, superstructure, flywheels, rollers);
autoChooser.addOption("Drive Straight", autoCommands.driveStraight());

// Set up feedforward characterization
Expand All @@ -185,6 +185,9 @@ private void configureAutos() {
flywheels,
flywheels::runCharacterizationVolts,
flywheels::getCharacterizationVelocity));

autoChooser.addOption(("Davis Ethical Auto"), autoCommands.davisEthicalAuto());
autoChooser.addOption("N5_S0_C0123", autoCommands.N5_S0_C012());
}

/**
Expand Down
96 changes: 90 additions & 6 deletions src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,16 +13,23 @@
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.subsystems.drive.Drive;
import org.littletonrobotics.frc2024.subsystems.drive.trajectory.HolonomicTrajectory;
import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.rollers.Rollers;
import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;

public class AutoCommands {
private final Drive drive;
private final Superstructure superstructure;
private final Flywheels flywheels;
private final Rollers rollers;

public AutoCommands(Drive drive, Superstructure superstructure) {
public AutoCommands(
Drive drive, Superstructure superstructure, Flywheels flywheels, Rollers rollers) {
this.drive = drive;
this.superstructure = superstructure;
this.flywheels = flywheels;
this.rollers = rollers;
}

private Command path(String pathName) {
Expand All @@ -47,12 +54,89 @@ private Command reset(String path) {

public Command davisEthicalAuto() {
return sequence(
reset("driveToCenterline4"),
path("driveToCenterline4"),
path("driveToCenterline3"),
path("driveToPodium"));
reset("davisEthicalAuto_driveToCenterline4"),
Commands.waitUntil(
() -> true
// () -> superstructure.atArmSetpoint() && flywheels.atSetpoint()
)
.andThen(rollers.feedShooterCommand().withTimeout(.3))
.deadlineWith(superstructure.aimCommand()),
path("davisEthicalAuto_driveToCenterline4")
.deadlineWith(
superstructure
.floorIntakeCommand()
.alongWith(rollers.floorIntakeCommand())
.withTimeout(3)
.andThen(superstructure.aimCommand())),
rollers.feedShooterCommand().withTimeout(.1).deadlineWith(superstructure.aimCommand()),
path("davisEthicalAuto_driveToCenterline3")
.deadlineWith(
superstructure
.floorIntakeCommand()
.alongWith(rollers.floorIntakeCommand())
.withTimeout(3)
.andThen(superstructure.aimCommand())),
rollers.feedShooterCommand().withTimeout(.1).deadlineWith(superstructure.aimCommand()),
path("davisEthicalAuto_driveToCenterline2")
.deadlineWith(
superstructure
.floorIntakeCommand()
.alongWith(rollers.floorIntakeCommand())
.withTimeout(3)
.andThen(superstructure.aimCommand())),
rollers.feedShooterCommand().withTimeout(.1).deadlineWith(superstructure.aimCommand()),
path("davisEthicalAuto_driveToPodium")
.deadlineWith(superstructure.floorIntakeCommand(), rollers.floorIntakeCommand()),
Commands.waitUntil(
() -> true
// superstructure::atArmSetpoint
)
.andThen(rollers.feedShooterCommand().withTimeout(.1)))
.deadlineWith(flywheels.shootCommand());
}

public Command N5_S0_C012() {
return sequence(
reset("N5-S0-C0123_driveToS0"),
Commands.waitUntil(
() -> true
// () -> superstructure.atArmSetpoint() && flywheels.atSetpoint()
)
.andThen(rollers.feedShooterCommand().withTimeout(.3))
.deadlineWith(superstructure.aimCommand()),
path("N5-S0-C0123_driveToS0")
.deadlineWith(
superstructure
.floorIntakeCommand()
.alongWith(rollers.floorIntakeCommand())
.withTimeout(3)
.andThen(superstructure.aimCommand())),
rollers.feedShooterCommand().withTimeout(.1).deadlineWith(superstructure.aimCommand()),
path("N5-S0-C0123_driveToC0")
.deadlineWith(
superstructure
.floorIntakeCommand()
.alongWith(rollers.floorIntakeCommand())
.withTimeout(3)
.andThen(superstructure.aimCommand())),
rollers.feedShooterCommand().withTimeout(.1).deadlineWith(superstructure.aimCommand()),
path("N5-S0-C0123_driveToC1")
.deadlineWith(
superstructure
.floorIntakeCommand()
.alongWith(rollers.floorIntakeCommand())
.withTimeout(3)
.andThen(superstructure.aimCommand())),
rollers.feedShooterCommand().withTimeout(.1).deadlineWith(superstructure.aimCommand()),
path("N5-S0-C0123_driveToC2")
.deadlineWith(superstructure.floorIntakeCommand(), rollers.floorIntakeCommand()),
Commands.waitUntil(
() -> true
// superstructure::atArmSetpoint
)
.andThen(rollers.feedShooterCommand().withTimeout(.1)))
.deadlineWith(flywheels.shootCommand());
}
;

public Command driveStraight() {
return reset("driveToCenterline4")
Expand Down
108 changes: 93 additions & 15 deletions ...ain/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
Expand Down Expand Up @@ -44,52 +43,131 @@ public class DriveTrajectories {

// Center intake locations
private static final double intakeOffset = 0.5;
private static final Pose2d[] intakingPoses = new Pose2d[5];
private static final Pose2d[] intakingCenterlinePoses = new Pose2d[5];

static {
// Find locations for intaking centerline gamepieces
for (int i = 0; i < 5; i++) {
Translation2d centerLineTranslation =
FieldConstants.StagingLocations.centerlineTranslations[i];
intakingPoses[i] =
intakingCenterlinePoses[i] =
new Pose2d(
centerLineTranslation.minus(new Translation2d(intakeOffset, 0)),
new Rotation2d(Math.PI));
}

// Add paths
// Davis Ethical Auto (5 Note)
paths.put(
"driveToCenterline4",
"davisEthicalAuto_driveToCenterline4",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(startingSourceFace)
.addTranslationWaypoint(new Translation2d(3.5, 2.5))
.addPoseWaypoint(intakingPoses[0])
.addPoseWaypoint(intakingCenterlinePoses[0])
.addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
.build()));

paths.put(
"driveToCenterline3",
"davisEthicalAuto_driveToCenterline3",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
.addTranslationWaypoint(new Translation2d(5.5, 1.4))
.addPoseWaypoint(intakingPoses[1])
.addPoseWaypoint(intakingCenterlinePoses[1])
.addTranslationWaypoint(new Translation2d(5.5, 1.4))
.addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
.build()));
paths.put(
"driveToPodium",
"davisEthicalAuto_driveToCenterline2",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
.addTranslationWaypoint(new Translation2d(1.6 - Units.inchesToMeters(20), 3.75))
.addTranslationWaypoint(
FieldConstants.Stage.center
.transformBy(new Transform2d(1.25, 0, Rotation2d.fromDegrees(-60)))
.getTranslation())
.addPoseWaypoint(intakingCenterlinePoses[2])
.addTranslationWaypoint(
FieldConstants.Stage.center
.transformBy(new Transform2d(1.25, 0, Rotation2d.fromDegrees(-60)))
.getTranslation())
.addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
.build()));
paths.put(
"davisEthicalAuto_driveToPodium",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
.addTranslationWaypoint(new Translation2d(2, 3.75))
.addPoseWaypoint(
getShootingPose(FieldConstants.StagingLocations.spikeTranslations[0])
.transformBy(new Transform2d(intakeOffset, 0, new Rotation2d())))
.build()));

// 5N-S0-C0123
paths.put(
"N5-S0-C0123_driveToS0",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(startingAmpFace)
.addPoseWaypoint(
getShootingPose(FieldConstants.StagingLocations.spikeTranslations[2]))
.build()));

paths.put(
"N5-S0-C0123_driveToC0",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(
getShootingPose(FieldConstants.StagingLocations.spikeTranslations[2]))
.addPoseWaypoint(intakingCenterlinePoses[4])
.addPoseWaypoint(
getShootingPose(
FieldConstants.Stage.center
.transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2)))
.getTranslation()))
.build()));

paths.put(
"N5-S0-C0123_driveToC1",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(
getShootingPose(
FieldConstants.Stage.center
.transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2)))
.getTranslation()))
.addPoseWaypoint(intakingCenterlinePoses[3])
.addPoseWaypoint(
getShootingPose(
FieldConstants.Stage.center
.transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2)))
.getTranslation()))
.build()));

paths.put(
"N5-S0-C0123_driveToC2",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(
getShootingPose(
FieldConstants.Stage.center
.transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2)))
.getTranslation()))
.addTranslationWaypoint(
FieldConstants.Stage.center
.transformBy(new Transform2d(1.25, 0, Rotation2d.fromDegrees(-60)))
.getTranslation())
.addPoseWaypoint(intakingCenterlinePoses[2])
.addTranslationWaypoint(
FieldConstants.Stage.center
.transformBy(new Transform2d(1.25, 0, Rotation2d.fromDegrees(-60)))
.getTranslation())
.addPoseWaypoint(
new Pose2d(
FieldConstants.StagingLocations.spikeTranslations[0].getX()
- Units.inchesToMeters(20),
FieldConstants.StagingLocations.spikeTranslations[0].getY(),
new Rotation2d(Math.PI)))
getShootingPose(
FieldConstants.Stage.center
.transformBy(new Transform2d(2, 0, new Rotation2d(Math.PI / 2)))
.getTranslation()))
.build()));
}

Expand Down
3 changes: 1 addition & 2 deletions .../java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,8 +91,7 @@ public static void main(String[] args) {
TrajectoryResponse response = service.generateTrajectory(request);
String error = response.getError().getReason();
if (error.length() > 0) {
System.err.println(
"Got error response for trajectory \"" + entry.getKey() + "\": " + error);
System.err.println("Error response for trajectory \"" + entry.getKey() + "\": " + error);
System.exit(1);
}
trajectory =
Expand Down
1 change: 0 additions & 1 deletion ...n/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/HolonomicTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
import java.io.InputStream;
import java.nio.file.Path;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.subsystems.drive.trajectory.TrajectoryGenerationHelpers;
import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.VehicleState;

@ExtensionMethod({TrajectoryGenerationHelpers.class})
Expand Down

0 comments on commit f2ffc27

Please sign in to comment.