diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/DriveSubsystem.java index ae1e596..ee4e9f0 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DriveSubsystem.java @@ -98,7 +98,7 @@ public void execute() { m_currentDriveYState = new TrapezoidProfile.State(s_drivetrain.getState().Pose.getY(), s_drivetrain.getState().Speeds.vyMetersPerSecond); m_currentTurnState = new TrapezoidProfile.State(s_drivetrain.getState().Pose.getRotation().getRadians(), s_drivetrain.getState().Speeds.omegaRadiansPerSecond); } */ - + // Get error which is the smallest distance between goal and measurement double errorBound = Math.PI; double measurement = s_drivetrain.getState().Pose.getRotation().getRadians(); @@ -117,7 +117,7 @@ public void execute() { m_currentDriveXState = s_driveProfile.calculate(dt, m_currentDriveXState, s_autoAlignTargetDriveX); m_currentDriveYState = s_driveProfile.calculate(dt, m_currentDriveYState, s_autoAlignTargetDriveY); m_currentTurnState = s_turnProfile.calculate(dt, m_currentTurnState, s_autoAlignTargetTurn); - + s_drivetrain.setControl( s_autoDrive .withTargetDirection(new Rotation2d(m_currentTurnState.position)) @@ -156,7 +156,7 @@ public void end(boolean interrupted) { private static CommandSwerveDrivetrain s_drivetrain; private static SwerveRequest.FieldCentric s_drive; private static FieldCentricWithPose s_autoDrive; - + private static DoubleSupplier s_driveRequest = () -> 0; private static DoubleSupplier s_strafeRequest = () -> 0; private static DoubleSupplier s_rotateRequest = () -> 0; @@ -227,6 +227,14 @@ public Pose2d getPose() { return s_drivetrain.getState().Pose; } + /** + * Checks if the robot is near the sourc + * @return The boolean value to check if the robot is near the source + */ + public boolean isNearSource() { + return ((getPose().getX() < 2.0) && ((getPose().getY() < 1.5) || getPose().getY() > 6.0)); + } + /** * Returns the location the robot should go to in order to align to the nearest reef pole */