diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8463dcf2..5c824254 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -307,7 +307,7 @@ private void configureBindings() { controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); - controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); @@ -346,7 +346,7 @@ private void configureBindings() { SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driveJoystick.getY() * -1, () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(steerJoystick::getX) + .withControllerRotationAxis(() -> {return steerJoystick.getX() * -1;}) .deadband(Constants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index e5261981..9ce8c70b 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; +import frc.robot.commands.auto.DoNothing; import frc.robot.commands.auto.shoot.BlueDepotShoot; import frc.robot.commands.auto.shoot.BlueMidShoot; import frc.robot.commands.auto.shoot.BlueOutpostShoot; @@ -30,7 +31,6 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.utils.logging.commands.DoNothingCommand; public class AutoChooser { @@ -106,11 +106,11 @@ private void populateChoosers() { private void populateCommandMap() { //if AutoEvent is not dependent on alliance color don't put a color commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.DEPOT_SIDE), - new DoNothingCommand()); + new DoNothing(turret, angler)); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MID), - new DoNothingCommand()); + new DoNothing(turret, angler)); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.OUTPOST_SIDE), - new DoNothingCommand()); + new DoNothing(turret, angler)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue), new BlueDepotShoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller)); //shoot depot blue diff --git a/src/main/java/frc/robot/commands/auto/DoNothing.java b/src/main/java/frc/robot/commands/auto/DoNothing.java index 1bce9784..b92bf2d1 100644 --- a/src/main/java/frc/robot/commands/auto/DoNothing.java +++ b/src/main/java/frc/robot/commands/auto/DoNothing.java @@ -1,13 +1,19 @@ package frc.robot.commands.auto; -import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.turret.RunTurretToRevLimit; +import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; -public class DoNothing extends LoggableCommand{ - public DoNothing() { - } +public class DoNothing extends LoggableParallelCommandGroup{ + public DoNothing(TurretSubsystem turret, AnglerSubsystem angler) { + super( + new LoggableParallelCommandGroup( + new RunTurretToRevLimit(turret), + new RunAnglerToReverseLimit(angler) + ) - @Override - public boolean isFinished() { - return true; + ); } } \ 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 13636392..952b4ade 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -170,4 +170,7 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; + + public static final double MAX_HUB_DISTANCE = 5.18; + public static final double MIN_HUB_DISTANCE = 1.42; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 8574fe51..b9c9f66a 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,15 +1,20 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; + +import java.util.ArrayList; + import org.dyn4j.UnitConversion; import org.littletonrobotics.junction.Logger; import frc.robot.RobotContainer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,8 +27,10 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; // Placeholder target poses until real field target values are finalized - private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, 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 BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, + 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"; @@ -39,20 +46,18 @@ public class ControllerSubsystem extends SubsystemBase { private static final String TARGET_HOPPER_SPEED_KEY = "controller/TargetHopperSpeed"; // Placeholder fixed-state settings. - 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); - private static final ShotTargets FIXED_2_TARGETS = - new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); + 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); + private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); // Placeholder pose-driven profiles. - private static final PoseControlProfile BLUE_HUB_PROFILE = - new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, 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 SHUTTLE_PROFILE = - new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0); + private static final PoseControlProfile BLUE_HUB_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, + 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 SHUTTLE_PROFILE = new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, + -14.0); private final SwerveSubsystem drivebase; private final RobotContainer robotContainer; @@ -62,7 +67,6 @@ public class ControllerSubsystem extends SubsystemBase { private ShotTargets activeTargets; private boolean driverActivatedShooting = false; - public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { this.drivebase = drivebase; this.robotContainer = robotContainer; @@ -78,10 +82,9 @@ public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContai public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); - updateStopDelayState(currentState); updateTargets(currentState, robotPose); - if(Constants.DEBUG){ + if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees); @@ -115,7 +118,8 @@ private Pose2d getRobotPose() { } private boolean shouldUseManualPose() { - // This can be confusing in case you're on the robot and have disabled the drivetrain + // This can be confusing in case you're on the robot and have disabled the + // drivetrain // Uncomment to control manually return false; } @@ -124,7 +128,7 @@ private Pose2d getManualPose() { double x = SmartDashboard.getNumber(MANUAL_POSE_X_KEY, 0.0); double y = SmartDashboard.getNumber(MANUAL_POSE_Y_KEY, 0.0); double r = SmartDashboard.getNumber(MANUAL_POSE_R_KEY, 0.0); - Logger.recordOutput("Manual Pose", new Pose2d(new Translation2d(x,y), new Rotation2d(r))); + Logger.recordOutput("Manual Pose", new Pose2d(new Translation2d(x, y), new Rotation2d(r))); return new Pose2d(x, y, new Rotation2d(r)); } @@ -144,21 +148,21 @@ private void updateTargets(ShootState state, Pose2d robotPose) { // No color, do nothing... useShotTargets(FIXED_TARGETS); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { - useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); } else { // Unknown color, do nothing... useShotTargets(FIXED_TARGETS); } } - case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose)); + case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPosePredictionCalculation(SHUTTLE_PROFILE.targetPose,robotPose))); } } private void updateStoppedTargets() { - //This makes the shooter wait half a second before stopping + // This makes the shooter wait half a second before stopping double shooterVelocity = stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS) ? 0.0 : activeTargets.shooterVelocityRpm; @@ -188,40 +192,66 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); - return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, + true); } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { - return robotPose.getTranslation().getDistance(targetPose.getTranslation()); + double distance = robotPose.getTranslation() + .getDistance(targetPose.getTranslation()); + if (distance > Constants.MAX_HUB_DISTANCE) { + return Constants.MAX_HUB_DISTANCE; + } else if (distance < Constants.MIN_HUB_DISTANCE) { + return Constants.MIN_HUB_DISTANCE; + } else { + return distance; + } + } + + private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPose) { + double flightTime = calculateFlightTime(robotPose.getTranslation() + .getDistance(targetPose.getTranslation())); + Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); + Pose2d predictedPose = robotPoseTransform + .plus(new Transform2d( + drivebase.getFieldVelocity().vxMetersPerSecond * flightTime, + drivebase.getFieldVelocity().vyMetersPerSecond * flightTime, + new Rotation2d())); + Logger.recordOutput("Predicted pose", predictedPose); + return predictedPose; + } + 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)) { - double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; - return - 0.169 * distance * distance - - 1.73 * distance - + 20.4; - } + double distance = (3.281 * computedDistanceMeters) - 2; + return 0.169 * distance * distance + - 1.73 * distance + + 20.4; + } 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; - return Math.floor( - 8.46 * distance * distance - - 237 * distance - - 1_380); - } + double distance = (3.81 * computedDistanceMeters) - 2; + return (8.46 * distance * distance + - 237 * distance + - 1_380); + } return profile.defaultShooterVelocityRpm; } private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), DriverStation.getAlliance().get()== DriverStation.Alliance.Blue))); + return Math.floor( + Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), + Robot.allianceColor().get() == DriverStation.Alliance.Blue))); } - //Getters for all the subsystems to set posistion. + // Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { return activeTargets.anglerAngleDegrees; } @@ -254,8 +284,7 @@ public boolean driverActivatedShootingEnabled() { return driverActivatedShooting; } - - //Class to save all the fixed targets + // Class to save all the fixed targets private static final class ShotTargets { private final double anglerAngleDegrees; private final double shooterVelocityRpm; @@ -280,7 +309,8 @@ private ShotTargets( } } - //Class to save all the target poses and each subsystem position at that point so we can calculate true values later on + // Class to save all the target poses and each subsystem position at that point + // so we can calculate true values later on private static final class PoseControlProfile { private final Pose2d targetPose; private final double defaultAnglerAngleDegrees; diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index 7e88c7b0..25ab9482 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -1,5 +1,11 @@ package frc.robot.utils.math; + +import org.littletonrobotics.junction.Logger; + +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.GameConstants; public class TurretCalculations { @@ -72,6 +78,7 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do // normalize angle between -PI and PI double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI)); + Logger.recordOutput("TurretPose", new Pose2d(new Translation2d(turretPosX,turretPosY), new Rotation2d(normalizedPanAngle))); return normalizedPanAngle; } 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