diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0edebafe..39b13d87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -72,6 +72,7 @@ import frc.robot.apriltags.ApriltagReading; import java.io.File; +import java.util.function.Supplier; import choreo.auto.AutoFactory; import choreo.auto.AutoRoutine; @@ -331,6 +332,7 @@ private void configureBindings() { Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } + } public void putShuffleboardCommands() { diff --git a/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java index 1fb25cdb..ced4fb44 100644 --- a/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java +++ b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java @@ -17,7 +17,7 @@ public DefaultTurretControl(TurretSubsystem turretSubsystem, ControllerSubsystem @Override public void execute() { - turretSubsystem.setAngle(controllerSubsystem.getTargetTurretAngleDegrees()); + new SetTurretAngle(turretSubsystem, controllerSubsystem.getTargetTurretAngleDegrees()); } @Override diff --git a/src/main/java/frc/robot/commands/turret/SetTurretAngle.java b/src/main/java/frc/robot/commands/turret/SetTurretAngle.java index 8b76311d..171ae7f9 100644 --- a/src/main/java/frc/robot/commands/turret/SetTurretAngle.java +++ b/src/main/java/frc/robot/commands/turret/SetTurretAngle.java @@ -1,5 +1,7 @@ package frc.robot.commands.turret; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.constants.GameConstants; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; @@ -7,24 +9,31 @@ * Runs the turret to the target angle */ public class SetTurretAngle extends LoggableCommand { - private final TurretSubsystem turret; - private double targetAngle; - - + private final TurretSubsystem turret; + private double targetAngle; + private boolean turretInRange; + + public SetTurretAngle(TurretSubsystem turret, double targetAngle) { + this.turret = turret; + this.targetAngle = targetAngle; + this.turretInRange = false; + addRequirements(turret); + } - public SetTurretAngle(TurretSubsystem turret, double targetAngle) { - this.turret = turret; - this.targetAngle = targetAngle; - addRequirements(turret); - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - turret.setAngle(targetAngle); + @Override + public void initialize() { + if ((targetAngle > GameConstants.TURRET_LEFT_ANGLE + 5) && + (targetAngle < GameConstants.TURRET_RIGHT_ANGLE - 5)) { + turretInRange = true; + } + } + + @Override + public void execute() { + SmartDashboard.putBoolean("WITHIN_SHOOTING_RANGE", turretInRange); + if (turretInRange) { + turret.setAngle(targetAngle); + } } @Override diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 614f0a90..07dfd82b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -57,6 +57,9 @@ public enum Mode { public static final double INITIAL_INTAKE_DEPLOYER_SPEED = 0.25; public static final double INITIAL_INTAKE_RETRACTION_SPEED = -0.1; + // Fixed Angles + public static final double FIXED_TURRET_ANGLE_1 = 0; + public static final double FIXED_TURRET_ANGLE_2 = 0; //Diags public static final double HOPPER_DIAGS_ENCODER = 1; @@ -147,8 +150,10 @@ public enum Mode { public static final double BLUE_HUB_Y_POSITION = 4.0345; public static final double RED_HUB_X_POSITION = 11.9154; public static final double RED_HUB_Y_POSITION = 4.0345; - public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware - public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .4; // needs value from hardware + public static final double BLUE_SHUTTLE_X_POSITION = 3; + public static final double BLUE_SHUTTLE_Y_POSITION = 4.0345; + public static final double RED_SHUTTLE_X_POSITION = 3; + public static final double RED_SHUTTLE_Y_POSITION = 4.0345; // Shift timings public static final int SHIFT_1_START = 130; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 6f2b0a28..3e86987e 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -5,6 +5,7 @@ import frc.robot.RobotContainer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -12,6 +13,7 @@ import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.math.TurretCalculations; public class ControllerSubsystem extends SubsystemBase { @@ -36,9 +38,9 @@ public class ControllerSubsystem extends SubsystemBase { private static final ShotTargets STOPPED_TARGETS = new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false); private static final ShotTargets FIXED_TARGETS = - new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); + new ShotTargets(10.0, 120.0, 15.0, 0.0, true, true); private static final ShotTargets FIXED_2_TARGETS = - new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); + new ShotTargets(22.0, 180.0, -15.0, 0.0, true, true); // Placeholder pose-driven profiles. private static final PoseControlProfile HUB_PROFILE = @@ -179,11 +181,24 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr return profile.defaultShooterVelocityRpm; } + // calculates turret angle from calculations private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - // TODO: Replace with distance turret angle calculation. - return profile.defaultTurretAngleDegrees; - } + + double targetAngle = 0; + + if (getCurrentShootState() == ShootState.SHOOTING_HUB) { + + targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlue()); + } else if (getCurrentShootState() == ShootState.SHUTTLING) { + + targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlue()); + } + return targetAngle; + } + //Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { return activeTargets.anglerAngleDegrees; @@ -261,4 +276,16 @@ private PoseControlProfile( this.defaultTurretAngleDegrees = defaultTurretAngleDegrees; } } + + /** + * Checks if the alliance is blue, defaults to false if alliance isn't + * available. + * + * @return true if the blue alliance, false if red. Defaults to false if none is + * available. + */ + private boolean isBlue() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Blue : false; + } } diff --git a/src/main/java/frc/robot/utils/math/LaunchCalculations.java b/src/main/java/frc/robot/utils/math/LaunchCalculations.java index 5ada137a..c4e1ec03 100644 --- a/src/main/java/frc/robot/utils/math/LaunchCalculations.java +++ b/src/main/java/frc/robot/utils/math/LaunchCalculations.java @@ -22,11 +22,6 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY // Target value -- what we're trying to find double shooterVelocity; // Initial velocity of the shooter -- related to flywheel speed - // calculates the position of the turret with respect to the origin using the robot center - // and the constant distance between the robot center and the turret. - double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - // Given values -- from Constants file double hubHeight = GameConstants.HUB_HEIGHT; // height of the top of the hub double shooterHeight = GameConstants.SHOOTER_HEIGHT; // height of shooter @@ -47,8 +42,8 @@ public static double calculateShooterVelocity(double robotPosX, double robotPosY } // Compute distance between hub and robot with Pythagorean Theorem - deltaDistance = Math.sqrt(Math.pow(hubPosY - turretPosY, 2) - + Math.pow(hubPosX - turretPosX, 2)); + deltaDistance = Math.sqrt(Math.pow(hubPosY - robotPosY, 2) + + Math.pow(hubPosX - robotPosX, 2)); // Compute the change in height between the shooter and the hub deltaHeight = hubHeight - shooterHeight; diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index d2aea690..9f1ca69f 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -30,11 +30,6 @@ public class TurretCalculations { // robotRotation is the angle between the horizontal (by the alliance side chute) and the robot public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { - // calculates the position of the turret with respect to the origin using the robot center - // and the constant distance between the robot center and the turret. - double turretPosX = robotPosX + GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - double turretPosY = robotPosY + GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET; - double hubPosX; double hubPosY; @@ -55,7 +50,43 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do * of the input numbers. * */ - double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX); + double panAngleUnadjusted = Math.atan2(hubPosY - robotPosY, hubPosX - robotPosX); + + /* + * Adjusts the pan angle to account for the robot's current rotation. We subtract the + * angle of the robot's rotation from the unadjusted angle of the turret to find the + * pan angle, which is the proper angle of the turret adjusted for the robot's rotation. + */ + double panAngle = panAngleUnadjusted - robotRotation; + + return panAngle; + + } + + // shuttlePosX and shuttlePosY are given values from the constants file -- gives the x and y positions of the shuttle site (in meters) + public static double calculateTurretShuttleAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) { + + double shuttlePosX; + double shuttlePosY; + + if (isBlueAlliance) { + // shuttle position determined by which alliance robot is on + shuttlePosX = GameConstants.BLUE_SHUTTLE_X_POSITION; + shuttlePosY = GameConstants.BLUE_SHUTTLE_Y_POSITION; + } else { + shuttlePosX = GameConstants.RED_SHUTTLE_X_POSITION; + shuttlePosY = GameConstants.RED_SHUTTLE_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 shuttle position + * and the x-distance between the robot and the shuttle position, 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(shuttlePosY - robotPosY, shuttlePosX - robotPosX); /* * Adjusts the pan angle to account for the robot's current rotation. We subtract the diff --git a/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java deleted file mode 100644 index e6692667..00000000 --- a/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.utils.calculations; - -public class LaunchCalculationsTest { - public LaunchCalculationsTest() { - - } -} diff --git a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java deleted file mode 100644 index 276eab71..00000000 --- a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.utils.calculations; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import frc.robot.utils.math.TurretCalculations; - -public class TurretCalculationsTest { - - static final double DELTA = .0348; // standard deviation of 2 degrees or .0348 radians - double[] xPos = {5.827, 13.850, 9.211, 1.211, 3.811}; - double[] yPos = {2.359, 7.921, 5.386, 0.857, 4.035}; - double[] robotRotation = {toRadians(30), toRadians(-53), toRadians(180), toRadians(45), toRadians(-45)}; - double[] panAngles = {toRadians(-18), toRadians(-102), toRadians(-217), toRadians(-2), toRadians(-1)}; - boolean[] isBlueAlliance = {false, true, false, true, true}; - - private int index; - - private double toRadians(double degree) { - return degree * Math.PI / 180; - } - - @Test - void index0Test() { - index = 0; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index1Test() { - index = 1; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index2Test() { - index = 2; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index3Test() { - index = 3; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index4Test() { - index = 4; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } -} \ No newline at end of file