diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5ab8e24..4479d1e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,6 +23,8 @@ public static enum Bot { public static final Bot currentBot; public static final String serialNumber; + public static final double BOT_WIDTH = 0.885; // metres + /** * This code determines what bot is being deployed and sets constants * accordingly. @@ -133,7 +135,7 @@ public static class ArmConstants { public static final double ARM_STOW_DEGREES = -83.0; public static final double ARM_STOW_2_DEGREES = -100.0; public static final double ARM_AMP_SHOOTING_DEGREES = -27.8; - public static final double ARM_SPEAKER_SHOOTING_DEGREES = -97.822 - 5 + 4 - 6 -1; + public static final double ARM_SPEAKER_SHOOTING_DEGREES = -97.822 - 5 + 4 - 6 - 1; public static final double ARM_INTAKE_DEGREES = -109 - 1.5; public static final int LEFT_MOTOR_ID = 5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2d6449..a6b05aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -146,6 +146,11 @@ public RobotContainer() { Autos.shootFromAmpSideAuto(drivetrain, arm, intakeShooter, leftHang, rightHang)); autoChooser.addOption("2+SourceSide", Autos.shootFromFarSideAuto(drivetrain, arm, intakeShooter, leftHang, rightHang)); + try { + autoChooser.addOption("3 note", + Autos.shoot3StartingAuto(drivetrain, arm, intakeShooter, vision, leftHang, rightHang, inputs)); + } catch (Exception e) { + } SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 7c36293..7887f70 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -26,6 +27,18 @@ * itself. */ public final class Autos { + + /** + * The translation to the preliminary position a side note pickup + *
+ * Relative to the robot which will be facing away from the speaker
+ */
+ private static final double speakerDepth = Units.inchesToMeters(36.125);
+ private static final Translation2d rightNotePickup = new Translation2d(
+ /* (dist from note to speaker front) / 2 - (bot offset from front of speaker) */
+ Units.inchesToMeters((114 - speakerDepth) / 2) - (Constants.BOT_WIDTH / 2),
+ /* lateral dist to note */
+ -Units.inchesToMeters(57));
final static double driveDistanceForNote1 = 1.1;
/** Example static factory for an autonomous command. */
@@ -140,6 +153,63 @@ public static Command shootFromFarSideAuto(SwerveDrivetrain drivetrain, Arm arm,
Hang leftHang, Hang rightHang) {
return shootFromSideAuto(drivetrain, arm, shooter, leftHang, rightHang, true);
}
+ public static Command shootSideNoteAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter,
+ Vision vision, Alliance alliance, boolean isAmpSide, ChassisDriveInputs inputs) {
+ // If on red, and going for amp side note (right side) keep translation
+ // If on red, and going for stage side note (left side) mirror translation y
+ // If on blue, and going for amp side note (left side) mirror translation y
+ // If on blue, and going for stage side note (right side) keep translation
+ // if (!vision.isEnabled())
+ // throw new UnsupportedOperationException("This auto requires vision!");
+
+ final int speakerTagId = (alliance == Alliance.Red) ? 4 : 7;
+ final Translation2d sideNoteTranslation = new Translation2d(rightNotePickup.getX(),
+ (alliance == Alliance.Red) == isAmpSide ? rightNotePickup.getY() : -rightNotePickup.getY());
+ return Commands.sequence(
+ intakeFromFloorStart(arm, shooter),
+ new AutoDriveTo(drivetrain, sideNoteTranslation),
+ new AutoDriveTo(drivetrain, new Translation2d(0, sideNoteTranslation.getY())),
+ intakeFromFloorEnd(arm, shooter),
+ // new AutoDriveTo(drivetrain, sideNoteTranslation.times(-1)),
+ new AutoDriveTo(drivetrain,
+ new Translation2d(-sideNoteTranslation.getX() * 2, -sideNoteTranslation.getY())),
+ // new FollowTag(drivetrain, vision, speakerTagId,
+ // new Translation2d(-speakerDepth - Constants.BOT_WIDTH / 2, 0)),
+ shootInSpeaker(drivetrain, arm, shooter, null, inputs));
+ }
+
+ public static Command shoot3StartingAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Vision vision,
+ Hang leftHanger, Hang rightHanger, ChassisDriveInputs inputs) throws Exception {
+ final Optional