diff --git a/AdvantageScopeSwerve.json b/AdvantageScopeSwerve.json index 4880e03..0c520a6 100644 --- a/AdvantageScopeSwerve.json +++ b/AdvantageScopeSwerve.json @@ -70,8 +70,6 @@ "/AdvantageKit", "/AdvantageKit/RealOutputs", "/AdvantageKit/RealOutputs/ShootOnTheMove/RobotHeading", - "/AdvantageKit/RealOutputs/ASCalibration", - "/AdvantageKit/RealOutputs/FieldSimulation", "/AdvantageKit/RealOutputs/FieldSimulation/Shooter" ] }, @@ -111,7 +109,7 @@ "type": "ghost", "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/TargetPose", "logType": "Pose2d", - "visible": true, + "visible": false, "options": { "model": "Crab Bot", "color": "#0000ff" @@ -131,7 +129,7 @@ "type": "ghost", "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/AimTarget", "logType": "Pose3d", - "visible": true, + "visible": false, "options": { "model": "Duck Bot", "color": "#00ffff" @@ -141,7 +139,7 @@ "type": "ghost", "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/AimTargetCorrected", "logType": "Pose3d", - "visible": true, + "visible": false, "options": { "model": "Duck Bot", "color": "#ff00ff" @@ -185,17 +183,17 @@ "cameraIndex": -1, "orbitFov": 50, "cameraPosition": [ - 10.378995714785386, - 9.101397756194608, - -0.10752311567010889 + 11.534767787918012, + 9.794331756031207, + -5.627252825256971 ], "cameraTarget": [ - 4.495231078701326, - -1.3207821976493825, - -0.22073703807687897 + 6.837520944122143, + 2.222662928789005, + -1.7627162173818105 ] }, - "controlsHeight": -570 + "controlsHeight": 519 }, { "type": 1, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 34c5163..e8bb9e4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,10 +2,39 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import swervelib.math.Matter; public final class Constants { + public static enum AimPoints { + RED_HUB(new Translation3d(11.938, 4.034536, 1.5748)), + RED_OUTPOST(new Translation3d(15.75, 7.25, 0)), + RED_FAR_SIDE(new Translation3d(15.75, 0.75, 0)), + + BLUE_HUB(new Translation3d(4.5974, 4.034536, 1.5748)), + BLUE_OUTPOST(new Translation3d(0.75, 0.75, 0)), + BLUE_FAR_SIDE(new Translation3d(0.75, 7.25, 0)); + + public final Translation3d value; + + private AimPoints(Translation3d value) { + this.value = value; + } + + public static final Translation3d getAllianceHubPosition() { + return DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? RED_HUB.value : BLUE_HUB.value; + } + + public static final Translation3d getAllianceOutpostPosition() { + return DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? RED_OUTPOST.value : BLUE_OUTPOST.value; + } + + public static final Translation3d getAllianceFarSidePosition() { + return DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? RED_FAR_SIDE.value : BLUE_FAR_SIDE.value; + } + } + public static final double ROBOT_MASS = Units.lbsToKilograms(120); // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms spark max velocity lag diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d87564a..6c16cc2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,15 +10,20 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import static edu.wpi.first.units.Units.Inches; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; import frc.robot.controls.DriverControls; import frc.robot.controls.OperatorControls; +import frc.robot.controls.PoseControls; import frc.robot.subsystems.HoodSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -42,6 +47,9 @@ public class RobotContainer { private final SendableChooser autoChooser; + // Track current alliance for change detection + private Alliance currentAlliance = Alliance.Red; + /** * The container for the robot. Contains subsystems, I/O devices, and commands. */ @@ -50,6 +58,20 @@ public RobotContainer() { configureBindings(); buildNamedAutoCommands(); + // Initialize alliance (default to red if not present) + onAllianceChanged(getAlliance()); + + // Set up trigger to detect alliance changes + new Trigger(() -> getAlliance() != currentAlliance) + .onTrue(Commands.runOnce(() -> onAllianceChanged(getAlliance())).ignoringDisable(true)); + + // Triggers for auto aim/pass poses + new Trigger(() -> isInAllianceZone()) + .onChange(Commands.runOnce(() -> onZoneChanged()).ignoringDisable(true)); + + new Trigger(() -> isOnAllianceOutpostSide()) + .onChange(Commands.runOnce(() -> onZoneChanged()).ignoringDisable(true)); + if (!Robot.isReal() || true) { DriverStation.silenceJoystickConnectionWarning(true); } @@ -72,7 +94,7 @@ private void configureBindings() { // Set up controllers DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, superstructure); OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, superstructure); - // PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); + PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); } private void buildNamedAutoCommands() { @@ -121,4 +143,64 @@ public Pose3d getAimDirection() { public Translation3d getAimPoint() { return superstructure.getAimPoint(); } + + public void setAimPoint(Translation3d aimPoint) { + superstructure.setAimPoint(aimPoint); + } + + private Alliance getAlliance() { + return DriverStation.getAlliance().orElse(Alliance.Red); + } + + private boolean isInAllianceZone() { + Alliance alliance = getAlliance(); + Distance blueZone = Inches.of(182); + Distance redZone = Inches.of(469); + + if (alliance == Alliance.Blue && drivebase.getPose().getMeasureX().lt(blueZone)) { + return true; + } else if (alliance == Alliance.Red && drivebase.getPose().getMeasureX().gt(redZone)) { + return true; + } + + return false; + } + + private boolean isOnAllianceOutpostSide() { + Alliance alliance = getAlliance(); + Distance midLine = Inches.of(158.84375); + + if (alliance == Alliance.Blue && drivebase.getPose().getMeasureY().lt(midLine)) { + return true; + } else if (alliance == Alliance.Red && drivebase.getPose().getMeasureY().gt(midLine)) { + return true; + } + + return false; + } + + private void onZoneChanged() { + if (isInAllianceZone()) { + superstructure.setAimPoint(Constants.AimPoints.getAllianceHubPosition()); + } else { + if (isOnAllianceOutpostSide()) { + superstructure.setAimPoint(Constants.AimPoints.getAllianceOutpostPosition()); + } else { + superstructure.setAimPoint(Constants.AimPoints.getAllianceFarSidePosition()); + } + } + } + + private void onAllianceChanged(Alliance alliance) { + currentAlliance = alliance; + + // Update aim point based on alliance + if (alliance == Alliance.Blue) { + superstructure.setAimPoint(Constants.AimPoints.BLUE_HUB.value); + } else { + superstructure.setAimPoint(Constants.AimPoints.RED_HUB.value); + } + + System.out.println("Alliance changed to: " + alliance); + } } diff --git a/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java b/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java index 8fd7849..7ad9db9 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java @@ -153,6 +153,10 @@ private Angle calculateRequiredHoodAngle(Distance distanceToTarget) { private static final InterpolatingDoubleTreeMap TIME_OF_FLIGHT_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries( Map.entry(1.0, 1.0), Map.entry(4.86, 1.5)); + // TODO: add more data points here. + // CLOSE: NEED + // MID: maybe good enough + // FAR: NEED // meters, RPS private static final InterpolatingDoubleTreeMap SHOOTING_SPEED_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries( diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 0764568..a55b994 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.Meter; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -109,19 +108,15 @@ public static Command fireFuel(SwerveSubsystem drivetrain, Superstructure supers return Commands.runOnce(() -> { SimulatedArena arena = SimulatedArena.getInstance(); - System.out.println("FIRE!"); - GamePieceProjectile fuel = new RebuiltFuelOnFly( drivetrain.getPose().getTranslation(), new Translation2d( superstructure.turret.turretTranslation.getX() * -1, superstructure.turret.turretTranslation.getY()), drivetrain.getSwerveDrive().getRobotVelocity(), - superstructure.getAimRotation3d().toRotation2d(), - Feet.of(superstructure.turret.turretTranslation.getZ()), + drivetrain.getPose().getRotation().rotateBy(superstructure.getAimRotation3d().toRotation2d()), + superstructure.turret.turretTranslation.getMeasureZ(), - // based on numbers from https://www.reca.lc/flywheel - // Adjust for simulation tuning // 0.5 times because we're applying spin to the fuel as we shoot it superstructure.getTangentialVelocity().times(0.5), superstructure.getHoodAngle()); diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index 61c239a..ecbcbef 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -87,45 +87,4 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur .ignoringDisable(true) .withName("OperatorControls.aimCommand")); } - - public static Command fireAlgae(SwerveSubsystem drivetrain, Superstructure superstructure) { - return Commands.runOnce(() -> { - System.err.println("FIRE!"); - - SimulatedArena arena = SimulatedArena.getInstance(); - - // Translation2d robotPosition, - // Translation2d shooterPositionOnRobot, - // ChassisSpeeds chassisSpeeds, - // Rotation2d shooterFacing, - // Distance initialHeight, - // LinearVelocity launchingSpeed, - // Angle shooterAngle - - GamePieceProjectile fuel = new RebuiltFuelOnFly( - drivetrain.getPose().getTranslation(), - new Translation2d(), - drivetrain.getSwerveDrive().getRobotVelocity().times(-1), - superstructure.getAimRotation3d().toRotation2d(), - Distance.ofBaseUnits(1, Feet), - - // based on numbers from https://www.reca.lc/flywheel - // superstructure.getTangentialVelocity().times(0.5), // adjust for simulation - // tuning - LinearVelocity.ofBaseUnits(5, FeetPerSecond), - superstructure.getHoodAngle()); - - // Configure callbacks to visualize the flight trajectory of the projectile - fuel.withProjectileTrajectoryDisplayCallBack( - // Callback for when the note will eventually hit the target (if configured) - (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot", - pose3ds.toArray(Pose3d[]::new)), - // Callback for when the note will eventually miss the target, or if no target - // is configured - (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot", - pose3ds.toArray(Pose3d[]::new))); - - arena.addGamePieceProjectile(fuel); - }).withName("Fire.Fuel"); - } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 83c5b9d..cf077a0 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; /** * Superstructure coordinates the shooter, turret, hood, and intake subsystems @@ -35,17 +36,17 @@ public class Superstructure extends SubsystemBase { private static final Angle HOOD_TOLERANCE = Degrees.of(2); // Triggers for readiness checks - public final Trigger isShooterAtSpeed; - public final Trigger isTurretOnTarget; - public final Trigger isHoodOnTarget; - public final Trigger isReadyToShoot; + private final Trigger isShooterAtSpeed; + private final Trigger isTurretOnTarget; + private final Trigger isHoodOnTarget; + private final Trigger isReadyToShoot; private AngularVelocity targetShooterSpeed = RPM.of(0); private Angle targetTurretAngle = Degrees.of(0); private Angle targetHoodAngle = Degrees.of(0); - // Hard coded red hub aim point - private Translation3d aimPoint = new Translation3d(Meter.of(11.902), Meter.of(4.031), Meter.of(0)); + // Default aim point is red hub + private Translation3d aimPoint = Constants.AimPoints.RED_HUB.value; public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, HopperSubsystem hopper, KickerSubsystem kicker) { diff --git a/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java b/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java index 7c9d58d..13bc4d2 100644 --- a/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java +++ b/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java @@ -1,9 +1,12 @@ package frc.robot.util.maplesim; import org.ironmaple.simulation.gamepieces.GamePieceProjectile; +import org.ironmaple.utils.FieldMirroringUtils; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; @@ -48,6 +51,18 @@ public RebuiltFuelOnFly( shooterAngle); super.withTouchGroundHeight(Inches.of(3).in(Meters)); + + super.withTargetPosition( + () -> FieldMirroringUtils.toCurrentAllianceTranslation(new Translation3d(11.938, 4.034536, 1.5748))); + + Logger.recordOutput("HubGoal", new Translation3d(11.938, 4.034536, 1.5748)); + + super.withTargetTolerance( + new Translation3d( + Inches.of(23.5).in(Meters), + Inches.of(23.5).in(Meters), + Inches.of(1).in(Meters))); + super.enableBecomesGamePieceOnFieldAfterTouchGround(); } } diff --git a/src/main/java/frc/robot/util/maplesim/RebuiltHub.java b/src/main/java/frc/robot/util/maplesim/RebuiltHub.java index 4a3af45..4ba007f 100644 --- a/src/main/java/frc/robot/util/maplesim/RebuiltHub.java +++ b/src/main/java/frc/robot/util/maplesim/RebuiltHub.java @@ -92,6 +92,11 @@ protected boolean checkCollision(GamePiece GamePiece) { + Math.pow(GamePiece.getPose3d().getZ() - position.getZ(), 2) < Math.pow(GoalRadius, 2); } + @Override + protected boolean checkRotation(GamePiece GamePiece) { + return true; + } + @Override protected void addPoints() { arena.addValueToMatchBreakdown(isBlue, "TotalFuelInHub", 1);