diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index b69bdc9b..6d2d1462 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -173,4 +173,8 @@ public enum Mode { public static final double MAX_HUB_DISTANCE = 5.18; public static final double MIN_HUB_DISTANCE = 1.42; public static final double COMPUTATED_DISTANCE_OFFSET = 2; + public static final double RED_SHUTTLING_TARGET_X_POSITION = 13.8; + public static final double BLUE_SHUTTLING_TARGET_X_POSITION = 2.8; + public static final double SHUTTLING_TARGET_LOWER_Y_POSITION = 1.6; + public static final double SHUTTLING_TARGET_HIGHER_Y_POSITION = 6.5; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 074d6163..60766ba6 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -31,8 +31,6 @@ public class ControllerSubsystem extends SubsystemBase { Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero); private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, Constants.RED_HUB_Y_POSITION, Rotation2d.kZero); - private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero); - private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY"; private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation"; @@ -57,9 +55,9 @@ public class ControllerSubsystem extends SubsystemBase { 14.0); private static final PoseControlProfile RED_HUB_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0, 14.0); - private static final PoseControlProfile RED_SHUTTLE_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 16.0, 90.0, + private static final PoseControlProfile RED_SHUTTLE_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 20.0, 90.0, -14.0); - private static final PoseControlProfile BLUE_SHUTTLE_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 16.0, 90.0, + private static final PoseControlProfile BLUE_SHUTTLE_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 20.0, 90.0, -14.0); private final SwerveSubsystem drivebase; @@ -202,9 +200,9 @@ private void useShotTargets(ShotTargets shotTargets) { private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) { double computedDistanceMeters = calculateDistanceMeters(state, robotPose, profile.targetPose); - double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); - double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); - double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); + double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, profile); + double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, profile); + double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, profile); return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true); } @@ -244,8 +242,8 @@ private double calculateFlightTime(double computedDistanceMeters) { return 0.208*computedDistanceMeters + 0.647; } - private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { - if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { + private double calculateAnglerAngleDegrees(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { + if (state == ShootState.SHOOTING_HUB) { double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; return 0.169 * distance * distance - 1.73 * distance @@ -254,21 +252,29 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo return profile.defaultAnglerAngleDegrees; } - private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { - if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; + private double calculateShooterVelocity(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { + double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; + if (state == ShootState.SHOOTING_HUB) { return (8.46 * distance * distance - 237 * distance - 1380); + }else if(state == ShootState.SHUTTLING){ + //Essentially random values needs to be tuned + return (-0.31 * distance * distance + -30.7 * distance + - 3838.7); } return profile.defaultShooterVelocityRpm; } - private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - return Math.floor( - Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), - robotPose.getRotation().getRadians(), - Robot.allianceColor().get() == DriverStation.Alliance.Blue))); + private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) { + if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING){ + return Math.floor( + Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), + Robot.allianceColor().get() == DriverStation.Alliance.Blue))); + } + return profile.defaultTurretAngleDegrees; } // Getters for all the subsystems to set posistion. @@ -348,4 +354,11 @@ private PoseControlProfile( this.defaultTurretAngleDegrees = defaultTurretAngleDegrees; } } + + public boolean isOnOpposingAllianceSide() { + return switch (Robot.allianceColor().get()) { + case Red -> getRobotPose().getX() < 4.6116; + case Blue -> getRobotPose().getX() > 11.9014; + }; + } } diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index 548d228d..11edb8ed 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -6,7 +6,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; +import frc.robot.constants.enums.ShootingState.ShootState; public class TurretCalculations { @@ -32,7 +34,7 @@ public class TurretCalculations { // all positions are in meters // robotRotation is in radians // Returns - turret angle in radians - public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { + public static double calculateTurretAngle(ShootState state, double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { // Get turret offset from robot center double turretOffsetX = GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; @@ -48,25 +50,30 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do double hubPosX; double hubPosY; - + if(state == ShootState.SHOOTING_HUB) { if (isBlueAlliance) { // hub position determined by which alliance robot is on - hubPosX = GameConstants.BLUE_HUB_X_POSITION; - hubPosY = GameConstants.BLUE_HUB_Y_POSITION; + hubPosX = Constants.BLUE_HUB_X_POSITION; + hubPosY = Constants.BLUE_HUB_Y_POSITION; } else { - hubPosX = GameConstants.RED_HUB_X_POSITION; - hubPosY = GameConstants.RED_HUB_Y_POSITION; + hubPosX = Constants.RED_HUB_X_POSITION; + hubPosY = Constants.RED_HUB_Y_POSITION; } - - /* - * This finds the unadjusted pan angle (assuming there is no robot rotation) using - * trigonometry. We take the arctangent of the y-distance beween the robot and the hub - * and the x-distance between the robot and the hub, giving us the unadjusted pan - * angle. The function atan2 ensures the sign of the angle is correct based on the signs - * of the input numbers. - * - */ - double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX); + }else{ + //Sets the hubPosX to be either on the blue or red side + //Sets the hubPoseY to be lower or upper depending on the Y position of the robot + hubPosX = isBlueAlliance ? Constants.BLUE_HUB_X_POSITION : Constants.RED_HUB_X_POSITION; + hubPosY = robotPosY > (Constants.FIELD_WIDTH / 2) ? Constants.SHUTTLING_TARGET_HIGHER_Y_POSITION : Constants.SHUTTLING_TARGET_LOWER_Y_POSITION; + } + /* + * This finds the unadjusted pan angle (assuming there is no robot rotation) using + * trigonometry. We take the arctangent of the y-distance beween the robot and the hub + * and the x-distance between the robot and the hub, giving us the unadjusted pan + * angle. The function atan2 ensures the sign of the angle is correct based on the signs + * of the input numbers. + * + */ + double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX); /* * Adjusts the pan angle to account for the robot's current rotation. We subtract the