From 7e9ba1a578b7445309c3567b19c1ecc48f8bed34 Mon Sep 17 00:00:00 2001 From: Ethan Torres Date: Tue, 27 Jan 2026 13:32:04 -0500 Subject: [PATCH] Cleaned Up Shoot On the Move --- src/main/java/frc/robot/Constants.java | 10 -- src/main/java/frc/robot/RobotContainer.java | 5 +- ...onary.java => PhysicsStationaryShoot.java} | 4 +- .../frc/robot/commands/ShootOnTheMove.java | 1 - src/main/java/frc/robot/subsystems/Hood.java | 139 +----------------- .../java/frc/robot/subsystems/Swerve.java | 7 + .../java/frc/robot/util/SOTMCalculator.java | 73 ++++++--- 7 files changed, 71 insertions(+), 168 deletions(-) rename src/main/java/frc/robot/commands/{AimandShootStationary.java => PhysicsStationaryShoot.java} (95%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1c22826..6c24159 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -313,16 +313,6 @@ public static class HoodConstants { .withFeedback(feedbackConfigs) .withMotorOutput(motorOutputConfigs) .withSoftwareLimitSwitch(softwareLimitSwitchConfigs); - - public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap(); - - static { - hoodAngleMap.put(1.5, 1.0); - hoodAngleMap.put(2.0, 2.0); - hoodAngleMap.put(2.5, 3.0); - hoodAngleMap.put(3.0, 4.0); - hoodAngleMap.put(3.53, 5.0); // random values (Distance, Rotations) - } } public static class SpindexerConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6574ec0..9f08824 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.SwerveConstants; -import frc.robot.commands.AimandShootStationary; +import frc.robot.commands.PhysicsStationaryShoot; import frc.robot.commands.MoveToFuel; import frc.robot.commands.ShootOnTheMove; import frc.robot.commands.TeleopSwerve; @@ -135,9 +135,8 @@ private void configureDriverBindings() { turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose)); hood.setDefaultCommand( - new AimandShootStationary( + new PhysicsStationaryShoot( shooter, hood, swerve::getRobotPose, () -> AllianceUtil.getHubPose())); - } private void configureOperatorBindings() { diff --git a/src/main/java/frc/robot/commands/AimandShootStationary.java b/src/main/java/frc/robot/commands/PhysicsStationaryShoot.java similarity index 95% rename from src/main/java/frc/robot/commands/AimandShootStationary.java rename to src/main/java/frc/robot/commands/PhysicsStationaryShoot.java index 3bd2310..948f721 100644 --- a/src/main/java/frc/robot/commands/AimandShootStationary.java +++ b/src/main/java/frc/robot/commands/PhysicsStationaryShoot.java @@ -18,13 +18,13 @@ /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class AimandShootStationary extends Command { +public class PhysicsStationaryShoot extends Command { private final Shooter shooter; private final Hood hood; private final Supplier robotPose; private final Supplier targetPose; - public AimandShootStationary( + public PhysicsStationaryShoot( Shooter shooter, Hood hood, Supplier robotPose, Supplier targetPose) { this.shooter = shooter; this.hood = hood; diff --git a/src/main/java/frc/robot/commands/ShootOnTheMove.java b/src/main/java/frc/robot/commands/ShootOnTheMove.java index 662352b..dd29c79 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMove.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMove.java @@ -70,7 +70,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - ChassisSpeeds fieldSpeeds = swerve.getChassisSpeeds(); ChassisSpeeds fieldAcceleration = fieldSpeeds.minus(previousSpeeds).div(0.020); diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 4a4b82e..3e95401 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -27,6 +27,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.HoodConstants; import frc.robot.Constants.TurretConstants; +import frc.robot.util.SOTMCalculator; + import java.util.function.Supplier; public class Hood extends SubsystemBase { @@ -35,8 +37,6 @@ public class Hood extends SubsystemBase { private SingleJointedArmSim hoodSim; - private final InterpolatingDoubleTreeMap hoodAngleMap = HoodConstants.hoodAngleMap; - private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0); private Angle targetAngle = HoodConstants.minAngle; @@ -74,95 +74,15 @@ public boolean atAngle(Angle target, Angle tolerance) { return Math.abs(getHoodAngle().minus(target).in(Degrees)) < tolerance.in(Degrees); } - public double getInterpolatedHoodAngle(double distanceMeters) { - return hoodAngleMap.get(distanceMeters); + public Angle getInterpolatedHoodAngle(double distanceMeters) { + return SOTMCalculator.hoodAngleMap.get(distanceMeters).getMeasure(); } - public double getInterpolatedHoodAngle(Pose2d poseA, Pose2d poseB) { + public Angle getInterpolatedHoodAngle(Pose2d poseA, Pose2d poseB) { double distance = poseA.getTranslation().getDistance(poseB.getTranslation()); - return hoodAngleMap.get(distance); + return SOTMCalculator.hoodAngleMap.get(distance).getMeasure(); } - // public Angle getPhysicsHoodAngle( - // Distance distance, - // Distance targetHeight, - // Distance shooterHeight, - // LinearVelocity exitVelocity) { - // double distanceMeters = distance.in(Meters); - // double targetHeightMeters = targetHeight.in(Meters); - // double shooterHeightMeters = shooterHeight.in(Meters); - // double exitVelocityMetersPerSec = exitVelocity.in(MetersPerSecond); - - // double vSq = exitVelocityMetersPerSec * exitVelocityMetersPerSec; - - // double dSq = distanceMeters * distanceMeters; - - // double dH = targetHeightMeters - shooterHeightMeters; - - // double g = 9.80665; - // // long physics equation i derived :D -> dont ask idk if it works - // double numerator = - // (vSq * distanceMeters) - // + Math.sqrt((vSq * vSq * dSq) - (g * dSq * ((2 * vSq * dH) + (g * dSq)))); - - // double denominator = g * dSq; - - // return Radians.of(Math.atan2(numerator, denominator)); - // } - - // public Angle getPhysicsHoodAngle( - // Distance distance, - // Distance targetHeight, - // Distance shooterHeight, - // LinearVelocity exitVelocity) { - // double distanceMeters = distance.in(Meters); - // double targetHeightMeters = targetHeight.in(Meters); - // double shooterHeightMeters = shooterHeight.in(Meters); - // double exitVelocityMetersPerSec = exitVelocity.in(MetersPerSecond); - - // double vSq = exitVelocityMetersPerSec * exitVelocityMetersPerSec; - - // double dH = targetHeightMeters - shooterHeightMeters; - - // double g = 9.80665; - - // double discriminant = (vSq * vSq) - g * (g * distanceMeters * distanceMeters + 2 * dH * vSq); - - // if (discriminant < 0) { - // return Radians.of(Double.NaN); - // } - - // Angle lowShotAngle = - // Radians.of(Math.atan((vSq - Math.sqrt(discriminant)) / (g * distanceMeters))); - // Angle highShotAngle = - // Radians.of(Math.atan((vSq + Math.sqrt(discriminant)) / (g * distanceMeters))); - - // Angle highHoodAngle = Degrees.of(90).minus(lowShotAngle); - // Angle lowHoodAngle = Degrees.of(90).minus(highShotAngle); - - // boolean lowValid = - // lowHoodAngle.in(Degrees) > HoodConstants.minAngle.in(Degrees) - // && lowHoodAngle.in(Degrees) < HoodConstants.maxAngle.in(Degrees); - - // boolean highValid = - // highHoodAngle.in(Degrees) > HoodConstants.minAngle.in(Degrees) - // && highHoodAngle.in(Degrees) < HoodConstants.maxAngle.in(Degrees); - - // SmartDashboard.putNumber("Hood/Physics/AngleChoices/LowHoodAngle", lowHoodAngle.in(Degrees)); - // SmartDashboard.putNumber("Hood/Physics/AngleChoices/HighHoodAngle", - // highHoodAngle.in(Degrees)); - - // if (lowValid && highValid) { - // return lowHoodAngle; - // } else if (lowValid) { - // return lowHoodAngle; - // } else if (highValid) { - // return highHoodAngle; - // } else { - // return Radians.of(Double.NaN); - // } - // } - public void stopHood() { hoodMotor.stopMotor(); } @@ -208,53 +128,6 @@ public Command aimForTarget( .withName("Aim Hood At Target"); } - // public Command aimWithPhysics( - // Supplier targetPoseSupplier, - // Supplier robotPoseSupplier, - // Supplier exitVelocitySupplier) { - // return run(() -> { - // Pose2d turretPose = - // robotPoseSupplier - // .get() - // .transformBy( - // new Transform2d( - // TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero)); - // Distance distance = - // Meters.of( - // turretPose - // .getTranslation() - // .getDistance(targetPoseSupplier.get().getTranslation())); - - // // Angle hoodAngle = - // // getPhysicsHoodAngle( - // // distance, - // // FieldConstants.hubHeight, - // // TurretConstants.robotToTurret.getMeasureZ(), - // // exitVelocitySupplier.get()); - // ShotSolution solution = - // solveShot( - // distance, - // FieldConstants.hubHeight, - // TurretConstants.robotToTurret.getMeasureZ(), - // exitVelocitySupplier.get()); - - // Angle hoodAngle = solution.hoodAngle(); - - // SmartDashboard.putNumber("Hood/Physics/ Chosen Angle ", hoodAngle.in(Degrees)); - // SmartDashboard.putNumber("Hood/Physics/distance ", distance.in(Meters)); - // SmartDashboard.putNumber( - // "Hood/Physics/D-Height ", - // FieldConstants.hubHeight - // .minus(TurretConstants.robotToTurret.getMeasureZ()) - // .in(Meters)); - // SmartDashboard.putNumber( - // "Hood/Physics/Velocity ", exitVelocitySupplier.get().in(MetersPerSecond)); - - // hoodMotor.setControl(motionMagicRequest.withPosition(hoodAngle.in(Rotations))); - // }) - // .withName("Aim Hood (Physics)"); - // } - @Override public void periodic() { hoodPosition.refresh(); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 8f6f057..55ad75f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -874,9 +874,16 @@ public ChassisSpeeds getChassisSpeeds() { @Logged(name = "Rotation") public Rotation2d getRotation() { +return stateCache.Pose.getRotation(); + } + + + @Logged(name = "Pigeon Rotation") + public Rotation2d getPigeonRotation() { return getPigeon2().getRotation2d(); } + @Logged(name = "Module States") public SwerveModuleState[] getModuleStates() { return stateCache.ModuleStates; diff --git a/src/main/java/frc/robot/util/SOTMCalculator.java b/src/main/java/frc/robot/util/SOTMCalculator.java index 37f66fb..6ed2d1e 100644 --- a/src/main/java/frc/robot/util/SOTMCalculator.java +++ b/src/main/java/frc/robot/util/SOTMCalculator.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Time; @@ -17,19 +19,51 @@ import frc.robot.subsystems.Turret; public class SOTMCalculator { - private static InterpolatingDoubleTreeMap hoodAngleMap = HoodConstants.hoodAngleMap; - private static InterpolatingDoubleTreeMap shooterSpeedMap = new InterpolatingDoubleTreeMap(); - private static InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); - private static Transform2d robotToTurret = - new Transform2d(TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero); - private static Time feedTime = Seconds.of(0.1353); + +public static InterpolatingTreeMap hoodAngleMap = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); + public static InterpolatingDoubleTreeMap shooterSpeedMap = new InterpolatingDoubleTreeMap(); + public static InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); + + static { + hoodAngleMap.put(0.8, Rotation2d.fromDegrees(16.0)); + hoodAngleMap.put(0.97, Rotation2d.fromDegrees(17.0)); + hoodAngleMap.put(1.1, Rotation2d.fromDegrees(18.0)); + hoodAngleMap.put(1.21, Rotation2d.fromDegrees(18.5)); + hoodAngleMap.put(1.34, Rotation2d.fromDegrees(19.0)); + hoodAngleMap.put(1.78, Rotation2d.fromDegrees(19.0)); + hoodAngleMap.put(2.17, Rotation2d.fromDegrees(24.0)); + hoodAngleMap.put(2.81, Rotation2d.fromDegrees(27.0)); + hoodAngleMap.put(3.82, Rotation2d.fromDegrees(29.0)); + hoodAngleMap.put(4.09, Rotation2d.fromDegrees(30.0)); + hoodAngleMap.put(4.40, Rotation2d.fromDegrees(31.0)); + hoodAngleMap.put(4.77, Rotation2d.fromDegrees(32.0)); + hoodAngleMap.put(5.57, Rotation2d.fromDegrees(32.0)); + hoodAngleMap.put(5.60, Rotation2d.fromDegrees(35.0)); // Random values + + shooterSpeedMap.put(1.5, 1.0); + shooterSpeedMap.put(2.0, 2.0); + shooterSpeedMap.put(2.5, 3.0); + shooterSpeedMap.put(3.0, 4.0); + shooterSpeedMap.put(3.53, 5.0); // random values (Distance, RPM) + + timeOfFlightMap.put(1.5, 1.0); + timeOfFlightMap.put(2.0, 1.0); + timeOfFlightMap.put(2.5, 1.0); + timeOfFlightMap.put(3.0, 1.0); + timeOfFlightMap.put(3.53, 1.0); // random values (Distance, seconds) + } + + private static Translation2d robotToTurret2d = TurretConstants.robotToTurret.toTranslation2d(); + public static Time feedTime = Seconds.of(0.1353); + public record ShootingParameters(double shooterSpeed, Angle turretAngle, Angle hoodAngle) {} public static ShootingParameters getParameters( Swerve swerve, Turret turret, Pose2d target, double fieldAccelX, double fieldAccelY) { Pose2d targetPose = target; - Pose2d turretPose = swerve.getRobotPose().transformBy(robotToTurret); + Translation2d turretPose = swerve.getRobotPose().getTranslation().plus(robotToTurret2d); ChassisSpeeds robotVelocity = swerve.getChassisSpeeds(); @@ -37,25 +71,26 @@ public static ShootingParameters getParameters( double turretVelocityX = robotVelocity.vxMetersPerSecond + robotVelocity.omegaRadiansPerSecond - * (robotToTurret.getY() * Math.cos(robotAngle) - - robotToTurret.getX() * Math.sin(robotAngle)); + * (robotToTurret2d.getY() * Math.cos(robotAngle) + - robotToTurret2d.getX() * Math.sin(robotAngle)); + double turretVelocityY = robotVelocity.vyMetersPerSecond + robotVelocity.omegaRadiansPerSecond - * (robotToTurret.getX() * Math.cos(robotAngle) - - robotToTurret.getY() * Math.sin(robotAngle)); + * (robotToTurret2d.getX() * Math.cos(robotAngle) + - robotToTurret2d.getY() * Math.sin(robotAngle)); Translation2d lookAheadPosition = targetPose.getTranslation(); - double turretToTargetDistance = lookAheadPosition.getDistance(turretPose.getTranslation()); + double turretToTargetDistance = lookAheadPosition.getDistance(turretPose); Angle turretAngle = turret.angleToFaceTarget(lookAheadPosition, swerve.getRobotPose()); - Angle hoodAngle = Rotations.of(hoodAngleMap.get(turretToTargetDistance)); + Rotation2d hoodAngle = hoodAngleMap.get(turretToTargetDistance); double shooterSpeed = shooterSpeedMap.get(turretToTargetDistance); double timeOfFlight = timeOfFlightMap.get(turretToTargetDistance); - double distance = lookAheadPosition.getDistance(turretPose.getTranslation()); + double distance = lookAheadPosition.getDistance(turretPose); for (int i = 0; i < 5; i++) { // + accelX * feedTime.in(Seconds) * 0.5 @@ -65,14 +100,14 @@ public static ShootingParameters getParameters( lookAheadPosition = targetPose.getTranslation().minus(new Translation2d(offsetX, offsetY)); - double newDistance = lookAheadPosition.getDistance(turretPose.getTranslation()); + double newDistance = lookAheadPosition.getDistance(turretPose); timeOfFlight = timeOfFlightMap.get(newDistance); Angle newTurretAngle = turret.angleToFaceTarget(lookAheadPosition, swerve.getRobotPose()); - Angle newHoodAngle = Rotations.of(hoodAngleMap.get(newDistance)); + Rotation2d newHoodAngle = hoodAngleMap.get(newDistance); double newShooterSpeed = shooterSpeedMap.get(newDistance); - boolean hasConverged = Math.abs(newDistance - distance) < 0.005; + boolean hasConverged = Math.abs(newDistance - distance) < 0.00005; turretAngle = newTurretAngle; hoodAngle = newHoodAngle; @@ -84,6 +119,6 @@ public static ShootingParameters getParameters( break; } } - return new ShootingParameters(shooterSpeed, turretAngle, hoodAngle); - } + return new ShootingParameters(shooterSpeed, turretAngle, hoodAngle.getMeasure()); + } }