diff --git a/src/main/deploy/tuning/arc.json b/src/main/deploy/tuning/arc.json old mode 100755 new mode 100644 index f05b6c4b..36c30ee9 --- a/src/main/deploy/tuning/arc.json +++ b/src/main/deploy/tuning/arc.json @@ -7,17 +7,20 @@ { // Shoot cmd "distance" : 1.48, - "velocity" : 54.0 + "velocity" : 54.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 1.60, - "velocity" : 56.0 + "velocity" : 56.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 1.80, - "velocity" : 61.0 + "velocity" : 61.0, + "hangtime" : 1.8 } ] }, @@ -27,22 +30,26 @@ { // Shoot cmd "distance" : 2.0, - "velocity" : 54.0 + "velocity" : 54.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 2.20, - "velocity" : 56.0 + "velocity" : 56.0, + "hangtime" : 1.8 }, { // Shoot mode "distance" : 2.40, - "velocity" : 59.0 + "velocity" : 59.0, + "hangtime" : 1.8 }, { // Shoot mode "distance" : 2.60, - "velocity" : 61.0 + "velocity" : 61.0, + "hangtime" : 1.8 } ] }, @@ -52,32 +59,38 @@ { // Shoot cmd "distance" : 2.8, - "velocity" : 56 + "velocity" : 56, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 3.0, - "velocity" : 58 + "velocity" : 58, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 3.2, - "velocity" : 59 + "velocity" : 59, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 3.4, - "velocity" : 61 + "velocity" : 61, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 3.60, - "velocity" : 63.0 + "velocity" : 63.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 3.8, - "velocity" : 64.0 + "velocity" : 64.0, + "hangtime" : 1.8 } ] }, @@ -87,22 +100,26 @@ { // Shoot cmd "distance" : 4.00, - "velocity" : 60.0 + "velocity" : 60.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 4.3, - "velocity" : 62.0 + "velocity" : 62.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 4.6, - "velocity" : 64.0 + "velocity" : 64.0, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 5.25, - "velocity" : 73.0 + "velocity" : 73.0, + "hangtime" : 1.8 } ] } diff --git a/src/main/flat.json b/src/main/flat.json deleted file mode 100755 index 47de3759..00000000 --- a/src/main/flat.json +++ /dev/null @@ -1,70 +0,0 @@ -{ - "name" : "flat", - "data" : [ - { - "hood" : 5.0, - "points" : [ - { - // Test mode - "distance" : 1.616, - "velocity" : 55.0 - }, - { - // Test mode - "distance" : 1.86, - "velocity" : 58.0 - }, - { - // Test mode - "distance" : 2.18, - "velocity" : 65.0 - } - ] - }, - { - "hood" : 35.0, - "points" : [ - { - // Test mode - "distance" : 2.47, - "velocity" : 62.0 - }, - { - // Test mode - "distance" : 2.77, - "velocity" : 64.0 - }, - { - // Test mode - "distance" : 3.06, - "velocity" : 68.0 - }, - { - // Test mode - "distance" : 3.35, - "velocity" : 73.0 - } - ] - }, - { - "hood" : 45.0, - "points" : [ - { - // Test mode - "distance" : 4.0, - "velocity" : 58.0 - }, - { - // Test mode - "distance" : 4.31, - "velocity" : 61.0 - }, - { - // Test mode - "distance" : 4.93, - "velocity" : 66.0 - } - ] - } - ] -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2d1dd1b0..e6bfdbbc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,10 +36,16 @@ public final class Constants { // Sets the currently running robot. Change to SIMBOT when running the // desktop physics simulation so AdvantageKit runs in SIM mode instead of // falling back to REPLAY. + //REMEMBER TO CHANGE BACK TO COMPETITION! private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; + public static final boolean shootOnMove = true; + + public static final double speedMultiplierShooting = 0.2; + public static final double speedMultiplierFerrying = 0.8; + public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11605c17..f5b50346 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -217,8 +217,8 @@ public RobotContainer() { if (hopper_ == null) { hopper_ = new Hopper(new HopperIO() {}); } - - // Force Load Apriltag Layout + + // Force Preload Static Apriltag Layout for (var tag : FieldConstants.layout.getTags()) { System.out.println("Tag Loaded: " + tag.ID); } @@ -230,7 +230,11 @@ public RobotContainer() { () -> -gamepad_.getRightX() ); - RobotState.initialize(drivebase_::getPose); + RobotState.initialize( + drivebase_::getPose, + drivebase_::getFieldChassisSpeeds, + shooter_::getTuning + ); // Initialize the visualizers. Mechanism3d.measured.zero(); @@ -285,19 +289,17 @@ private void configureBindings() { // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().or(operatorGamepad_.leftTrigger()).whileTrue(RobotCommands.intake(intake_, hopper_)); - // While the right trigger is held, we will shoot into the hub or ferry. Binding A to the shaking of the shooter. + // While the right trigger is held, we will shoot into the hub or ferry. gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()) .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_, gamepad_.a().or(operatorGamepad_.a()))); - // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.idleCommand()); - - //While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. - + // While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. operatorGamepad_.x().whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed())); - operatorGamepad_.y().whileTrue(RobotCommands.ejectUp(shooter_, hopper_)); + // When the shooter isnt shooting, stow it. + shooter_.setDefaultCommand(shooter_.idleCommand()); + // Bind dashboard button to refreshing the tuning. var refreshTuningButton = new LoggedNetworkBoolean("/Tuning/RefreshTuning", false); new Trigger(refreshTuningButton::get) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 2ebd84c4..cb2436be 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -4,37 +4,60 @@ import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.ShooterTuning; import frc.robot.util.LoggedTracer; +import frc.robot.util.VirtualTarget; /** * Object that calculates robot-wide values that change periodically, * for example, distance to a field element. */ public class RobotState { - private static Supplier pose = () -> Pose2d.kZero; + private static Supplier pose = null; + private static Supplier speed = null; + private static Supplier tuning = null; - @AutoLogOutput - private static Distance hubDistance = Meters.zero(); + private static boolean updatedRealHub = false; + private static boolean updatedVirtualHub = false; + private static boolean updatedVirtualFerry = false; + + private static Pose2d currentPose = Pose2d.kZero; + private static Translation2d hubTranslation = Translation2d.kZero; + private static Distance allianceWall = Meters.zero(); - private static Rotation2d rotationToHub = Rotation2d.kZero; private static boolean inAllianceZone = false; + private static Distance hubDistance = Meters.zero(); + private static Rotation2d rotationToHub = Rotation2d.kZero; + + private static Distance virtualHubDistance = Meters.zero(); + private static Rotation2d rotationToVirtualHub = Rotation2d.kZero; + + private static Distance virtualFerryDistance = Meters.zero(); + private static Rotation2d rotationToVirtualFerry = Rotation2d.kZero; + /** * This method supplies the object with the information it needs for its calculations. * @param poseSupplier */ - public static void initialize(Supplier poseSupplier) { + public static void initialize( + Supplier poseSupplier, + Supplier speedSupplier, + Supplier tuningSupplier + ) { pose = poseSupplier; + speed = speedSupplier; + tuning = tuningSupplier; } /** @@ -42,42 +65,134 @@ public static void initialize(Supplier poseSupplier) { */ public static void periodic() { LoggedTracer.reset(); - - Pose2d currentPose = pose.get(); - Translation2d hubTranslation = + // If not initialized, do nothing. + if ( + pose == null || + speed == null || + tuning == null + ) return; + + updatedRealHub = false; + updatedVirtualHub = false; + updatedVirtualFerry = false; + + currentPose = pose.get(); + + hubTranslation = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? ShooterConstants.Positions.blueHubPose : ShooterConstants.Positions.redHubPose; - - hubDistance = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); - Logger.recordOutput("RobotState/hubDistance", hubDistance) ; - Translation2d translationToHub = hubTranslation.minus(currentPose.getTranslation()); - - rotationToHub = new Rotation2d(translationToHub.getX(), translationToHub.getY()); - - Distance allianceWall = + allianceWall = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? ShooterConstants.Positions.blueAllianceWall : ShooterConstants.Positions.redAllianceWall; inAllianceZone = currentPose.getMeasureX().minus(allianceWall).abs(Meters) < ShooterConstants.Positions.spinUpZone.in(Meters); - LoggedTracer.record("RobotState"); + LoggedTracer.record("RobotState/InAllianceZone"); + + // Log Outputs + Logger.recordOutput("RobotState/HubDistance", hubDistance); + Logger.recordOutput("RobotState/VirtualHubDistance", virtualHubDistance); + } + + private static void updateRealHub() { + LoggedTracer.reset(); + + hubDistance = Meters.of(currentPose.getTranslation().getDistance(hubTranslation)); + + Translation2d translationToHub = hubTranslation.minus(currentPose.getTranslation()); + + rotationToHub = new Rotation2d(translationToHub.getX(), translationToHub.getY()); + + updatedRealHub = true; + + LoggedTracer.record("RobotState/UpdateRealHub"); + } + + private static void updateVirtualHub() { + LoggedTracer.reset(); + + Pose2d currentPose = pose.get(); + + Translation2d virtualHub = + VirtualTarget.getVirtualTargetFromTarget(currentPose, speed.get(), hubTranslation, tuning.get(), ShooterConstants.hangtimeLoopPasses); + + virtualHubDistance = Meters.of(currentPose.getTranslation().getDistance(virtualHub)); + + Translation2d translationToTarget = virtualHub.minus(currentPose.getTranslation()); + rotationToVirtualHub = new Rotation2d(translationToTarget.getX(), translationToTarget.getY()); + + Logger.recordOutput("RobotState/VirtualHub", virtualHub); + + updatedVirtualHub = true; + + LoggedTracer.record("RobotState/UpdateVirtualHub"); + } + + private static void updateVirtualFerry() { + LoggedTracer.reset(); + + Translation2d rightTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueTargetRight + : ShooterConstants.Positions.redTargetRight; + + Translation2d leftTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueTargetLeft + : ShooterConstants.Positions.redTargetLeft; + + Translation2d realTarget = currentPose.getY() < ShooterConstants.Positions.centerLineY + ? rightTarget + : leftTarget; + + Translation2d virtualTarget = + VirtualTarget.getVirtualTargetFromTarget(currentPose, speed.get(), realTarget, tuning.get(), ShooterConstants.hangtimeLoopPasses); + + Translation2d botToTarget = virtualTarget.minus(currentPose.getTranslation()); + + virtualFerryDistance = Meters.of(virtualTarget.getDistance(currentPose.getTranslation())); + rotationToVirtualFerry = new Rotation2d(botToTarget.getX(), botToTarget.getY()); + + updatedVirtualFerry = true; + + LoggedTracer.record("RobotState/UpdateVirtualFerry"); } public static Distance hubDistance() { + if (!updatedRealHub) updateRealHub(); return hubDistance; } - @AutoLogOutput public static Rotation2d rotationToHub() { + if (!updatedRealHub) updateRealHub(); return rotationToHub; } - @AutoLogOutput + public static Distance virtualHubDistance() { + if (!updatedVirtualHub) updateVirtualHub(); + return virtualHubDistance; + } + + public static Rotation2d rotationToVirtualHub() { + if (!updatedVirtualHub) updateVirtualHub(); + return rotationToVirtualHub; + } + + public static Distance virtualFerryDistance() { + if (!updatedVirtualFerry) updateVirtualFerry(); + return virtualFerryDistance; + } + + public static Rotation2d rotationToVirtualFerry() { + if (!updatedVirtualFerry) updateVirtualFerry(); + return rotationToVirtualFerry; + } + public static boolean inAllianceZone() { return inAllianceZone; } diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 4155d330..e0e5a152 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -134,6 +134,20 @@ public static Command joystickDrive() { return joystickDrive(drive_, xSupplier_, ySupplier_, omegaSupplier_); } + /** + * Field relative drive command using joystick for linear control and PID for + * angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, + * or controlling + * absolute rotation with a joystick. + * This is preconfigured with {@link #configure(Drive, DoubleSupplier, DoubleSupplier, DoubleSupplier)} + */ + public static Command joystickDriveAtAngle(Supplier rotationSupplier, DoubleSupplier throttle) { + if (!configured) throw new IllegalStateException("DriveCommands joystickDriveAtAngle called without first configuring!"); + + return joystickDriveAtAngle(drive_, xSupplier_, ySupplier_, rotationSupplier, throttle); + } + /** * Field relative drive command using joystick for linear control and PID for * angular control. @@ -145,7 +159,7 @@ public static Command joystickDrive() { public static Command joystickDriveAtAngle(Supplier rotationSupplier) { if (!configured) throw new IllegalStateException("DriveCommands joystickDriveAtAngle called without first configuring!"); - return joystickDriveAtAngle(drive_, xSupplier_, ySupplier_, rotationSupplier); + return joystickDriveAtAngle(rotationSupplier, () -> 1.0); } /** @@ -197,7 +211,9 @@ public static Command joystickDriveAtAngle( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - Supplier rotationSupplier) { + Supplier rotationSupplier, + DoubleSupplier throttle + ) { // Create PID controller ProfiledPIDController angleController = new ProfiledPIDController( @@ -220,9 +236,11 @@ public static Command joystickDriveAtAngle( // Convert to field relative speeds & send command ChassisSpeeds speeds = new ChassisSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega); + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * throttle.getAsDouble(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec() * throttle.getAsDouble(), + omega + ); + boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 5833c1e4..d9f793c3 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -1,20 +1,13 @@ package frc.robot.commands.robot; -import static edu.wpi.first.units.Units.Meters; - import java.util.function.BooleanSupplier; -import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; 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; import frc.robot.RobotState; import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; @@ -33,20 +26,17 @@ public class RobotCommands { */ private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive, Trigger shakeTrigger) { BooleanSupplier aimedAtHub = - () -> drive.rotationIsNear(RobotState.rotationToHub(), ShooterConstants.aimingTolerance); + () -> drive.rotationIsNear(RobotState.rotationToVirtualHub(), ShooterConstants.aimingTolerance); return Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - () -> 0, - () -> 0, - () -> RobotState.rotationToHub() - ).finallyDo(drive::stopWithX), + Constants.shootOnMove + ? DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualHub, () -> Constants.speedMultiplierShooting) + : DriveCommands.joystickDriveAtAngle(drive, () -> 0.0, () -> 0.0, RobotState::rotationToHub, () -> 1.0), Commands.repeatingSequence( Commands.waitUntil(aimedAtHub).deadlineFor(shooter.spinUpForDistance(RobotState::hubDistance)), - shooter.shootAtDistance(RobotState::hubDistance, hopper, intake, shakeTrigger) + shooter.shootAtDistance(RobotState::virtualHubDistance, hopper, intake, shakeTrigger) .until(() -> !aimedAtHub.getAsBoolean()) - ) + ).alongWith(Commands.run(() -> Logger.recordOutput("Shooter/AimedAtHub", aimedAtHub.getAsBoolean()))) ); } @@ -58,36 +48,12 @@ private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem * @return */ private static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive, Trigger shakeTrigger) { - Translation2d rightTarget = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueTargetRight - : ShooterConstants.Positions.redTargetRight; - - Translation2d leftTarget = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueTargetLeft - : ShooterConstants.Positions.redTargetLeft; - - Supplier target = - () -> drive.getPose().getY() < ShooterConstants.Positions.centerLineY - ? rightTarget - : leftTarget; - - Supplier targetDistance = () -> Meters.of(target.get().getDistance(drive.getPose().getTranslation())); - - Supplier targetingAngle = () -> { - var botToTarget = target.get().minus(drive.getPose().getTranslation()); - return new Rotation2d(botToTarget.getX(), botToTarget.getY()); - }; - - return DriveCommands.joystickDriveAtAngle(targetingAngle) + return DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualFerry, () -> Constants.speedMultiplierFerrying) .alongWith( - shooter.shootAtDistance(targetDistance, hopper, intake, shakeTrigger), - Commands.runOnce(() -> { - Logger.recordOutput("Ferry/Target", target.get()); - Logger.recordOutput("Ferry/IsFerrying", true); - }) - ).finallyDo(i -> Logger.recordOutput("Ferry/IsFerrying", false)); + shooter.shootAtDistance(RobotState::virtualFerryDistance, hopper, intake, shakeTrigger), + Commands.runOnce(() -> Logger.recordOutput("Ferry/IsFerrying", true)) + ) + .finallyDo(i -> Logger.recordOutput("Ferry/IsFerrying", false)); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java old mode 100755 new mode 100644 diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b20fead8..5e770b4e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -20,7 +20,6 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -38,9 +37,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; -import frc.robot.RobotState; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterTuning.ShooterParams; @@ -167,7 +164,7 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { setHoodAngle(hoodTarget); } - private ShooterTuning getTuning() { + public ShooterTuning getTuning() { return tunings_.get(tuningIndex_); } @@ -189,35 +186,6 @@ public Command reloadTunings() { }).ignoringDisable(true); } - /** - * The command that the shooter can run whenever its not shooting to manage - * things like going to different hood angles to get ready to shoot, - * or lowering the hood under the trench. - * @return A command that does so. - */ - public Command awaitShooting(Supplier robotPose) { - return runDynamicSetpoints( - // () -> RotationsPerSecond.of( - // RobotState.inAllianceZone() - // ? getTuning().getShooterParams(RobotState.hubDistance().in(Meters)).velocity - // : 0.0 - // ), - () -> RotationsPerSecond.of(0.0) , - () -> { - Pose2d pose = robotPose.get(); - Pose2d nearestTrench = pose.nearest(FieldConstants.trenches); - Distance nearestDistance = Meters.of(pose.getTranslation().getDistance(nearestTrench.getTranslation())); - - if (nearestDistance.lte(ShooterConstants.allowedTrenchDistance)) { - return Degrees.zero(); - } - - var params = getTuning().getShooterParams(RobotState.hubDistance().in(Meters)); - return Degrees.of(params.hood); - } - ); - } - public Command runToSetpointsCmd(AngularVelocity vel, Angle pos) { return startEnd(() -> setSetpoints(vel, pos), this::stopShooter).withName("Set Shooter Setpoints"); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 981fd534..81d1f9c0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -33,21 +33,17 @@ public class ShooterConstants { public static final AngularVelocity shooterTolerance = RotationsPerSecond.of(0.1); public static final AngularVelocity ejectVelocity = RotationsPerSecond.of(-20); - public static final Angle aimingTolerance = Degrees.of(5); + public static final Angle aimingTolerance = Degrees.of(8.0); public static final Distance allowedTrenchDistance = Meters.of(1.0); + public static final int hangtimeLoopPasses = 5; // for a for loop in calculating distance from the hub, as you need the distance to get the hangtime, but you also need the hangtime to get the distance public static final double timeBeforeShoot = 0.2; public static final Angle hoodParkedAngle = Degrees.of(5.0) ; public static final Angle hoodMaxAngle = Degrees.of(75.0) ; public static final Angle hoodMinAngle = Degrees.of(0.0) ; - public class FerryPositions{ - public static final Translation2d blueOutpostTarget= new Translation2d(2.135, 1.639); - public static final Translation2d redOutpostTarget= new Translation2d(14.0,6.0); - } - public class PID { // shooter public static final double shooterkP = 0.45; @@ -57,78 +53,62 @@ public class PID { public static final double shooterkA = 0.0; public static final double shooterkG = 0.0; public static final double shooterkS = 0.0; - } - - public class MotionMagic { - - // shooter - public static final double shooterkMaxVelocity = 1000.0; - public static final double shooterkMaxAcceleration = 3000.0; - public static final double shooterkJerk = 0.0; - } + } - public class SoftwareLimits { - public static final double hoodMaxAngle = 0.0; - public static final double hoodMinAngle = 0.0; - } + public class MotionMagic { - public class Positions { - // Field Positions - public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); - public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); + // shooter + public static final double shooterkMaxVelocity = 1000.0; + public static final double shooterkMaxAcceleration = 3000.0; + public static final double shooterkJerk = 0.0; + } - public static final Distance allianceZone = Meters.of(4.5974); - public static final Distance spinUpZone = Meters.of(6.0); + public class Positions { + // Field Positions + public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); + public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); - public static final Distance blueAllianceWall = Meters.of(0); - public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength()); + public static final Distance allianceZone = Meters.of(4.5974); + public static final Distance spinUpZone = Meters.of(6.0); - public static final Translation2d blueTargetLeft = new Translation2d(2,6); - public static final Translation2d blueTargetRight = new Translation2d(2,2); + public static final Distance blueAllianceWall = Meters.of(0); + public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength()); - public static final Translation2d redTargetLeft = new Translation2d(14,6); - public static final Translation2d redTargetRight = new Translation2d(14,2); + public static final Translation2d blueTargetLeft = new Translation2d(2,6); + public static final Translation2d blueTargetRight = new Translation2d(2,2); - public static final double centerLineY = 4.034536; + public static final Translation2d redTargetLeft = new Translation2d(14,6); + public static final Translation2d redTargetRight = new Translation2d(14,2); - // Hood Setpoints - public static final double hoodLOW = 0; - public static final double hoodMEDIUM = 1; - public static final double hoodHIGH = 2; - - public enum HubDistance { - LOW(Meters.of(2)), // 0-2 m - MEDIUM(Meters.of(3)), // in - HIGH(Meters.of(4)); // 96+ in + public static final double centerLineY = FieldConstants.layout.getFieldWidth() / 2.0; + + public enum HubDistance { + LOW(Meters.of(2)), // 0-2 m + MEDIUM(Meters.of(3)), // in + HIGH(Meters.of(4)); // 96+ in - private final Distance maxDistance; + private final Distance maxDistance; - private HubDistance(Distance maxDistance) { - this.maxDistance = maxDistance; - } + private HubDistance(Distance maxDistance) { + this.maxDistance = maxDistance; + } - public Distance maxDistance() { return maxDistance; } + public Distance maxDistance() { return maxDistance; } - public static HubDistance fromDistance(Distance pos) { - for(HubDistance vol : values()) { - if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) { - return vol; - } - } - return HubDistance.HIGH; + public static HubDistance fromDistance(Distance pos) { + for(HubDistance vol : values()) { + if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) { + return vol; + } } + return HubDistance.HIGH; } + } } public class HoodPWMs { - public static final int hoodLeftPWMPort = 2; - public static final int hoodRightPWMPort = 0; + public static final int hoodLeftPWMPort = 2; + public static final int hoodRightPWMPort = 0; } - public class ferryPositions{ - public static final Translation2d blueOutpostFerryTarget= new Translation2d(2.136,1.935); //ferry target for the blue outpost - public static final Translation2d redOutpostFerryTarget= new Translation2d(14.0,6.0); //ferry target for the red outpost - public static final Translation2d blueDepotFerryTarget= new Translation2d(2.0,6.0); //ferry target for the blue depot - public static final Translation2d redDepotFerryTarget= new Translation2d(14.0,2.0); //ferry target for the red depot - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java index 5d54a557..3812284b 100755 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java @@ -19,21 +19,25 @@ public class ShooterParams { public double dist ; public double hood ; public double velocity ; + public double hangtime; - public ShooterParams(double d, double h, double v) { + public ShooterParams(double d, double h, double v, double ht) { dist = d ; hood = h ; velocity = v ; + hangtime = ht ; } } ; public static class OneTuningValue { public final double dist_ ; public final double vel_ ; + public final double hangtime_ ; - public OneTuningValue(double d, double v) { + public OneTuningValue(double d, double v, double ht) { dist_ = d ; vel_ = v ; + hangtime_ = ht ; } } @@ -41,13 +45,17 @@ private class OneHoodTuning { public double hood_ ; public double min_dist_ ; public double max_dist_ ; - public InterpolatingDoubleTreeMap map_ ; + public InterpolatingDoubleTreeMap velmap_ ; + public InterpolatingDoubleTreeMap hangmap_ ; - public OneHoodTuning(double h, double[] d, double[] v) { + + public OneHoodTuning(double h, double[] d, double[] v, double[] ht) { hood_ = h ; - map_ = new InterpolatingDoubleTreeMap() ; + velmap_ = new InterpolatingDoubleTreeMap() ; + hangmap_ = new InterpolatingDoubleTreeMap() ; for(var i = 0 ; i < d.length ; i++) { - map_.put(d[i], v[i]) ; + velmap_.put(d[i], v[i]) ; + hangmap_.put(d[i], ht[i]) ; } min_dist_ = d[0] ; @@ -97,7 +105,9 @@ private void readTuningData(String filename) { JsonNode pt = pointsArray.get(i) ; double distance = pt.get("distance").asDouble() ; double velocity = pt.get("velocity").asDouble() ; - values[i] = new OneTuningValue(distance, velocity) ; + double hangtime = pt.get("hangtime").asDouble() ; + + values[i] = new OneTuningValue(distance, velocity, hangtime) ; } addOneHood(hood, values) ; @@ -111,9 +121,10 @@ private void readTuningData(String filename) { public ShooterParams getShooterParams(double dist) { int h = getHoodIndex(dist) ; - var ret = new ShooterParams(dist, settings_.get(h).hood_, settings_.get(h).map_.get(dist)) ; + var ret = new ShooterParams(dist, settings_.get(h).hood_, settings_.get(h).velmap_.get(dist), settings_.get(h).hangmap_.get(dist)) ; Logger.recordOutput("ShooterTuning/hood", ret.hood); Logger.recordOutput("ShooterTuning/velocity", ret.velocity); + Logger.recordOutput("ShooterTuning/hangtime", ret.hangtime); return ret ; } @@ -159,6 +170,7 @@ protected void addOneHood(double hood, OneTuningValue[] values) { double [] dist = new double[values.length] ; double [] vels = new double[values.length] ; + double [] hangtimes = new double[values.length] ; for(int i = 0 ; i < values.length ; i++) { var v = values[i] ; @@ -167,8 +179,9 @@ protected void addOneHood(double hood, OneTuningValue[] values) { } dist[i] = v.dist_ ; vels[i] = v.vel_ ; + hangtimes[i] = v.hangtime_; } - settings_.add(new OneHoodTuning(hood, dist, vels)); + settings_.add(new OneHoodTuning(hood, dist, vels, hangtimes)); settings_.sort((s1, s2) -> { return Double.compare(s1.hood_, s2.hood_); diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index 2a1fbb4d..7c3e9f90 100755 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -101,6 +101,7 @@ public void periodic() { // Iterate cameras for logging and pose estimations. for (int cam = 0; cam < io_.length; cam++) { + // Activate disconnected alert. alerts_[cam].set(!inputs_[cam].connected); diff --git a/src/main/java/frc/robot/util/VirtualTarget.java b/src/main/java/frc/robot/util/VirtualTarget.java new file mode 100644 index 00000000..7a549c1e --- /dev/null +++ b/src/main/java/frc/robot/util/VirtualTarget.java @@ -0,0 +1,53 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; +import frc.robot.subsystems.shooter.ShooterTuning; + +public class VirtualTarget { + /** + * Gets the translation of the virtual target based on a target, and the current drivebase velocity. + * + * In order to shoot, we need to find the tuning constants for a shot at the distance from the virtual target, + * which includes the hangtime of a shot of that distance. + * + * In order to calculate the virtual target, you need this hangtime which can only be found after the virtual target + * is calculated, in order to fix this, this function is recursive, and based on the number of loop passes, it will + * push the virtual target closer and closer to where it should be. + * + * This has been tested to work at slow velocities. + * + * @param drive + * @param target + * @param tuning + * @param loopPasses + * @return + */ + public static Translation2d getVirtualTargetFromTarget(Pose2d pose, ChassisSpeeds speeds, Translation2d target, ShooterTuning tuning, int loopPasses){ + Distance distToHub; + + if(loopPasses == 0) { + distToHub = Meters.of(target.getDistance(pose.getTranslation())); + } else{ + distToHub = Meters.of( + getVirtualTargetFromTarget(pose, speeds, target, tuning, loopPasses - 1).getDistance(pose.getTranslation()) + ); + } + + Time hangTime = Seconds.of(tuning.getShooterParams(distToHub.in(Meters)).hangtime); + + return target.minus( + new Translation2d( + MetersPerSecond.of(speeds.vxMetersPerSecond).times(hangTime), + MetersPerSecond.of(speeds.vyMetersPerSecond).times(hangTime) + ) + ); + } +}