diff --git a/src/main/java/frc/constants/field/Field.java b/src/main/java/frc/constants/field/Field.java index 1da6c8b565..3733fbec95 100644 --- a/src/main/java/frc/constants/field/Field.java +++ b/src/main/java/frc/constants/field/Field.java @@ -31,6 +31,8 @@ public static boolean isFieldConventionAlliance() { public static final double DEPOT_Y_AXIS_LENGTH_METERS = 1.0668; public static final double DEPOT_X_AXIS_LENGTH_METERS = 0.6858; public static final double TOWER_Y_AXIS_LENGTH_METERS = 0.82; + public static final double TRENCH_Y_AXIS_LENGTH_METERS = 1.668; + public static final double TRENCH_BAR_X_AXIS_LENGTH_METERS = 0.101; private static final Translation2d OUTPOST_MIDDLE = new Translation2d(0, 0.67); public static final Translation2d TOWER_MIDDLE = new Translation2d(0.53, 3.75); diff --git a/src/main/java/frc/robot/statemachine/ShootingCalculations.java b/src/main/java/frc/robot/statemachine/ShootingCalculations.java index ce0abc6249..be1914b19a 100644 --- a/src/main/java/frc/robot/statemachine/ShootingCalculations.java +++ b/src/main/java/frc/robot/statemachine/ShootingCalculations.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.interpolation.InverseInterpolator; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.constants.field.Field; +import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.robot.statemachine.shooterstatehandler.ShootingParams; import frc.robot.subsystems.constants.hood.HoodConstants; import frc.robot.subsystems.constants.turret.TurretConstants; @@ -21,6 +22,7 @@ public class ShootingCalculations { HoodConstants.MINIMUM_POSITION, TurretConstants.MIN_POSITION, new Rotation2d(), + false, new Translation2d(), Field.getHubMiddle() ); @@ -40,18 +42,12 @@ private static ShootingParams calculateShootingParams( // Calculate distance from turret to target Translation2d fieldRelativeTurretTranslation = getFieldRelativeTurretPosition(robotPose); double distanceFromTurretToTargetMeters = targetTranslation.getDistance(fieldRelativeTurretTranslation); - // Split Robot's Speeds - Translation2d robotTranslationalVelocity = new Translation2d( - fieldRelativeSpeeds.vxMetersPerSecond, - fieldRelativeSpeeds.vyMetersPerSecond - ); - // Turret Field Relative Velocity - Translation2d turretTangentialVelocity = TurretConstants.TURRET_POSITION_RELATIVE_TO_ROBOT.toTranslation2d() - .rotateBy(Rotation2d.kCCW_90deg) - .times(gyroYawAngularVelocity.getRadians()) - .rotateBy(robotPose.getRotation()); - Translation2d turretFieldRelativeVelocity = robotTranslationalVelocity.plus(turretTangentialVelocity); + Translation2d turretFieldRelativeVelocity = calculateFieldRelativeTurretVelocities( + robotPose, + fieldRelativeSpeeds, + gyroYawAngularVelocity + ); Translation2d turretPredictedPose = getPredictedTurretPose( fieldRelativeTurretTranslation, @@ -73,10 +69,21 @@ private static ShootingParams calculateShootingParams( Rotation2d hoodTargetPosition = hoodInterpolation.get(distanceFromTurretPredictedPoseToHub); Rotation2d flywheelTargetRPS = flywheelInterpolation.get(distanceFromTurretPredictedPoseToHub); + boolean isHoodTargetCorrect = true; + + if ( + ShootingChecks.isHoodInDangerOfCrushing(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity) + && hoodTargetPosition.getDegrees() < ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH.getDegrees() + ) { + hoodTargetPosition = ShooterConstants.MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH; + isHoodTargetCorrect = false; + } + Logger.recordOutput(LOG_PATH + "/turretFieldRelativePose", new Pose2d(fieldRelativeTurretTranslation, new Rotation2d())); Logger.recordOutput(LOG_PATH + "/turretTarget", turretTargetPosition); Logger.recordOutput(LOG_PATH + "/turretTargetVelocityRPS", turretTargetVelocityRPS); Logger.recordOutput(LOG_PATH + "/hoodTarget", hoodTargetPosition); + Logger.recordOutput(LOG_PATH + "/isHoodTargetCorrect", isHoodTargetCorrect); Logger.recordOutput(LOG_PATH + "/flywheelTarget", flywheelTargetRPS); Logger.recordOutput(LOG_PATH + "/distanceFromTarget", distanceFromTurretToTargetMeters); Logger.recordOutput(LOG_PATH + "/distanceFromTargetPredict", distanceFromTurretPredictedPoseToHub); @@ -88,6 +95,7 @@ private static ShootingParams calculateShootingParams( hoodTargetPosition, turretTargetPosition, turretTargetVelocityRPS, + isHoodTargetCorrect, turretPredictedPose, targetTranslation ); @@ -123,6 +131,25 @@ private static ShootingParams calculatePassingParams( ); } + public static Translation2d calculateFieldRelativeTurretVelocities( + Pose2d robotPose, + ChassisSpeeds fieldRelativeSpeeds, + Rotation2d gyroYawAngularVelocity + ) { + // Split Robot's Speeds + Translation2d robotTranslationalVelocity = new Translation2d( + fieldRelativeSpeeds.vxMetersPerSecond, + fieldRelativeSpeeds.vyMetersPerSecond + ); + + // Turret Field Relative Velocity + Translation2d turretTangentialVelocity = TurretConstants.TURRET_POSITION_RELATIVE_TO_ROBOT.toTranslation2d() + .rotateBy(Rotation2d.kCCW_90deg) + .times(gyroYawAngularVelocity.getRadians()) + .rotateBy(robotPose.getRotation()); + return robotTranslationalVelocity.plus(turretTangentialVelocity); + } + private static Translation2d getPredictedTurretPoseByFlightTime( Translation2d turretPose, Translation2d turretVelocities, @@ -166,6 +193,16 @@ public static Translation2d getFieldRelativeTurretPosition(Pose2d robotPose) { ); } + public static Translation2d getTurretPositionWhenHoodCloses( + Translation2d currentTurretPosition, + Translation2d fieldRelativeTurretVelocities + ) { + return new Translation2d( + currentTurretPosition.getX() + fieldRelativeTurretVelocities.getX() * ShooterConstants.TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC, + currentTurretPosition.getY() + fieldRelativeTurretVelocities.getY() * ShooterConstants.TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC + ); + } + public static double getDistanceFromHub(Translation2d pose) { return Field.getHubMiddle().getDistance(pose); } diff --git a/src/main/java/frc/robot/statemachine/ShootingChecks.java b/src/main/java/frc/robot/statemachine/ShootingChecks.java index f9cfc45720..206ea96fcc 100644 --- a/src/main/java/frc/robot/statemachine/ShootingChecks.java +++ b/src/main/java/frc/robot/statemachine/ShootingChecks.java @@ -1,12 +1,17 @@ package frc.robot.statemachine; import edu.wpi.first.math.MathUtil; +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 frc.constants.field.AllianceSide; import frc.constants.field.Field; import frc.robot.Robot; import frc.robot.statemachine.shooterstatehandler.ShooterConstants; import frc.utils.HubUtil; +import frc.utils.math.AngleTransform; +import frc.utils.math.FieldMath; import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; @@ -78,6 +83,61 @@ private static boolean isHoodAtPosition(Rotation2d hoodPosition, Rotation2d targ return isHoodAtPosition; } + private static boolean isTurretPositionAlignedWithTrenchOnYAxis(Translation2d allianceRelativeTurretPosition) { + return allianceRelativeTurretPosition.getY() + <= Field.getTrenchMiddle(AllianceSide.OUTPOST).getY() + (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2) + || allianceRelativeTurretPosition.getY() + >= Field.getTrenchMiddle(AllianceSide.DEPOT).getY() - (Field.TRENCH_Y_AXIS_LENGTH_METERS / 2); + } + + private static Boolean isTurretUnderTrench(Translation2d allianceRelativeTurretPosition) { + return (MathUtil.isNear( + allianceRelativeTurretPosition.getX(), + Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)).getX(), + ShooterConstants.DISTANCH_FROM_TRENCH_CENTER_TO_CLOSE_HOOD + ) && isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition)); + } + + private static Boolean isRobotGoingUnderTrench(Pose2d robotPose, Translation2d fieldRelativeTurretVelocities) { + Boolean isPredictedTurretPoseUnderTrench; + Translation2d currentTurretPosition = ShootingCalculations.getFieldRelativeTurretPosition(robotPose); + Translation2d allianceRelativePredictedTurretPositionWhenHoodCloses = ShootingCalculations + .getTurretPositionWhenHoodCloses(currentTurretPosition, fieldRelativeTurretVelocities); + Translation2d allianceRelativeOutpostTrenchMiddle = Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.OUTPOST)); + Translation2d allianceRelativeDepotTrenchMiddle = Field.getAllianceRelative(Field.getTrenchMiddle(AllianceSide.DEPOT)); + Translation2d allianceRelativeTurretPosition = Field.getAllianceRelative(currentTurretPosition); + + boolean isTurretUnderTrench = isTurretUnderTrench(allianceRelativeTurretPosition); + boolean DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses = isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativeTurretPosition) + || isTurretPositionAlignedWithTrenchOnYAxis(allianceRelativePredictedTurretPositionWhenHoodCloses); + + if (allianceRelativeTurretPosition.getX() < allianceRelativeOutpostTrenchMiddle.getX()) { + isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + > allianceRelativeDepotTrenchMiddle.getX() + && DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses; + } else { + isPredictedTurretPoseUnderTrench = allianceRelativePredictedTurretPositionWhenHoodCloses.getX() + < allianceRelativeDepotTrenchMiddle.getX() + && DoesTurretAlignWithTrenchOnYAxisUntilHoodCloses; + } + Logger.recordOutput(shootingChecksLogPath + "/AreWeTryingToGoUnderTrench", isPredictedTurretPoseUnderTrench); + Logger.recordOutput( + shootingChecksLogPath + "/allianceRelativePredictedTurretPositionWhenHoodCloses", + new Pose2d(allianceRelativePredictedTurretPositionWhenHoodCloses, Rotation2d.k180deg) + ); + return isPredictedTurretPoseUnderTrench || isTurretUnderTrench; + } + + public static Boolean isHoodInDangerOfCrushing(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Rotation2d gyroYawAngularVelocity) { + Translation2d fieldRelativeTurretVelocities = ShootingCalculations + .calculateFieldRelativeTurretVelocities(robotPose, fieldRelativeSpeeds, gyroYawAngularVelocity); + Boolean isRobotHeadingTowardsOurAllianceTrench = isRobotGoingUnderTrench(robotPose, fieldRelativeTurretVelocities); + Pose2d robotPoseReversed = FieldMath.mirror(robotPose, true, true, AngleTransform.INVERT); + Translation2d fieldRelativeTurretVelocitiesReversed = new Translation2d().minus(fieldRelativeTurretVelocities); + Boolean isRobotHeadingTowardsOpposingAllianceTrench = isRobotGoingUnderTrench(robotPoseReversed, fieldRelativeTurretVelocitiesReversed); + return (isRobotHeadingTowardsOurAllianceTrench || isRobotHeadingTowardsOpposingAllianceTrench); + } + private static boolean isReadyToShoot( Robot robot, Rotation2d flywheelVelocityToleranceRPS, @@ -118,7 +178,13 @@ private static boolean isReadyToShoot( logPath ); - boolean isReadyToShoot = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition && isTurretWithinDistance + boolean isHoodPositionCorrect = ShootingCalculations.getShootingParams().isTargetHoodPositionCorrect(); + + boolean isReadyToShoot = isAtTurretAtTarget + && isFlywheelReadyToShoot + && isHoodAtPosition + && isTurretWithinDistance + && isHoodPositionCorrect /* && isPoseReliable */; Logger.recordOutput(shootingChecksLogPath + "/IsReadyToShoot", isReadyToShoot); @@ -159,10 +225,13 @@ private static boolean canContinueShooting( ); boolean isTurretWithinDistance = isWithinDistance(predictedTurretPosition, maxShootingDistanceFromTargetMeters, target, logPath); + boolean isHoodPositionCorrect = ShootingCalculations.getShootingParams().isTargetHoodPositionCorrect(); + boolean canContinueShooting = isAtTurretAtTarget && isFlywheelReadyToShoot && isHoodAtPosition - && isTurretWithinDistance /* && isPoseReliable */; + && isTurretWithinDistance + && isHoodPositionCorrect/* && isPoseReliable */; Logger.recordOutput(shootingChecksLogPath + "/CanContinueShooting", canContinueShooting); return canContinueShooting; diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java index ebf7303bf3..10cefe04ac 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShooterConstants.java @@ -1,12 +1,18 @@ package frc.robot.statemachine.shooterstatehandler; import edu.wpi.first.math.geometry.Rotation2d; +import frc.constants.field.Field; import frc.utils.LoggedNetworkRotation2d; public class ShooterConstants { public static final Rotation2d DEFAULT_FLYWHEEL_ROTATIONS_PER_SECOND = Rotation2d.fromRotations(10); + public static final double TIME_TO_CLOSE_HOOD_WITH_BUFFER_SEC = 0.5; + public static final double DISTANCH_FROM_TRENCH_CENTER_TO_CLOSE_HOOD = Field.TRENCH_BAR_X_AXIS_LENGTH_METERS * 2; + + public static final Rotation2d MIN_HOOD_POSITION_TO_GO_UNDER_TRENCH = Rotation2d.fromDegrees(50); + public static final LoggedNetworkRotation2d turretCalibrationAngle = new LoggedNetworkRotation2d( "Tunable/TurretAngle", Rotation2d.fromDegrees(0.0) diff --git a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java index 2e8ede11a6..82a7415ec4 100644 --- a/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java +++ b/src/main/java/frc/robot/statemachine/shooterstatehandler/ShootingParams.java @@ -9,6 +9,7 @@ public record ShootingParams( Rotation2d targetHoodPosition, Rotation2d targetTurretPosition, Rotation2d targetTurretVelocityRPS, + boolean isTargetHoodPositionCorrect, Translation2d predictedTurretPoseWhenBallLands, Translation2d targetLandingPosition ) {} diff --git a/src/main/java/frc/robot/subsystems/constants/hood/HoodCalibration.md b/src/main/java/frc/robot/subsystems/constants/hood/HoodCalibration.md index 00c425cce1..63b54b9152 100644 --- a/src/main/java/frc/robot/subsystems/constants/hood/HoodCalibration.md +++ b/src/main/java/frc/robot/subsystems/constants/hood/HoodCalibration.md @@ -15,6 +15,7 @@ Hood calibrations - [x] Min position - [x] Forward limit - [x] Backwards limit +- [ ] Min angle for trench #### FeedForward - [x] kS @@ -38,4 +39,5 @@ Hood calibrations - [x] Current threshold to reset #### General Constants -- [x] Arm length \ No newline at end of file +- [x] Arm length +- [ ] Time to close hood \ No newline at end of file