diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b9ac010..d35a1277 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.RunAnglerToReverseLimit; import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.turret.AimTurret; import frc.robot.commands.turret.RunTurretToFwdLimit; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.commands.turret.SetTurretAngle; @@ -63,6 +64,7 @@ import frc.robot.apriltags.TCPApriltagIo; import java.io.File; +import java.util.function.Supplier; /** * This class is where the bulk of the robot should be declared. Since @@ -224,6 +226,8 @@ private void configureBindings() { Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } + + turretSubsystem.setDefaultCommand(new AimTurret(turretSubsystem, drivebase::getPose, getShootingState())); } public void putShuffleboardCommands() { diff --git a/src/main/java/frc/robot/commands/turret/AimTurret.java b/src/main/java/frc/robot/commands/turret/AimTurret.java new file mode 100644 index 00000000..8653dcc9 --- /dev/null +++ b/src/main/java/frc/robot/commands/turret/AimTurret.java @@ -0,0 +1,99 @@ +package frc.robot.commands.turret; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.constants.GameConstants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.utils.math.TurretCalculations; +import java.util.function.Supplier; + +/** + * Default command that aims the turret based on robot pose and shooting state. + * + */ +public class AimTurret extends LoggableCommand { + + private final TurretSubsystem turret; + private final Supplier poseSupplier; + private final ShootingState shootingState; + private final boolean isBlueAlliance; + + public AimTurret(TurretSubsystem turret, Supplier poseSupplier, ShootingState shootingState) { + this.turret = turret; + this.poseSupplier = poseSupplier; + this.shootingState = shootingState; + this.isBlueAlliance = isBlue(); + addRequirements(turret); + } + + @Override + public void execute() { + Pose2d robotPose = poseSupplier.get(); + ShootState state = shootingState.getShootState(); + handleState(state, robotPose); + } + + @Override + public boolean isFinished() { + return false; + } + + private void handleState(ShootState state, Pose2d robotPose) { + switch (state) { + case STOPPED: + turret.stopMotors(); + // set to fixed turret angle at the various fixed states + case FIXED: + new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_1); + case FIXED_2: + new SetTurretAngle(turret, GameConstants.FIXED_TURRET_ANGLE_2); + // auto aim to hub + case SHOOTING_HUB: + AutoShoot(turret, robotPose); + // auto aim to shuttle site + case SHUTTLING: + ShuttleShoot(turret, robotPose); + } + } + + /** + * Moves turret to position automatically calculated based on angle from the + * hub. + * + * @param turret + * @param robotPose + */ + private void AutoShoot(TurretSubsystem turret, Pose2d robotPose) { + double targetAngle = TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlueAlliance); + new SetTurretAngle(turret, targetAngle); + } + + /** + * Moves turret to shuttling position based on angle from shuttling position + * + * @param turret + * @param robotPose + */ + private void ShuttleShoot(TurretSubsystem turret, Pose2d robotPose) { + double targetAngle = TurretCalculations.calculateTurretShuttleAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), isBlueAlliance); + new SetTurretAngle(turret, targetAngle); + } + + /** + * 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; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index a350e28c..928da131 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 = 10; public static final double INITIAL_INTAKE_RETRACTION_SPEED = -10; + // Fixed Angles + public static final double FIXED_TURRET_ANGLE_1 = 0; + public static final double FIXED_TURRET_ANGLE_2 = 0; //Timeouts public static final double SPIN_TIMEOUT = 5; @@ -135,6 +138,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 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; 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 diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index d2aea690..efee4832 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -68,4 +68,45 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do } + // 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) { + + // 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 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 - turretPosY, shuttlePosX - turretPosX); + + /* + * 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; + + } + }