diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a5a0407..4566648 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -181,8 +181,8 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final float TURRET_FORWARD_LIMIT = 130; // TODO: find - public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 130; // TODO: find + public static final double TURRET_REVERSE_LIMIT = 0; // TODO: find // deadzones public static final double HARD_DEADZONE_LEFT = 0.0; diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index d0c706a..ed01eb8 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -61,21 +61,12 @@ public class AimToCenter extends CommandBase { } /** - * Checks if in hardware deadzone (due to mechanical limitations). + * Checks if in deadzone. * @param angle Angle to check. - * @return True if in hardware deadzone. + * @return True if in deadzone. */ - public static boolean isHardwareDeadzone(double angle) { - return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT)); - } - - /** - * Checks if in digital deadzone (due to climber). - * @param angle Angle to check. - * @return True if in digital deadzone. - */ - public static boolean isDigitalDeadzone(double angle) { - return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT)); + public static boolean isDeadzone(double angle) { + return !((ShooterConstants.TURRET_REVERSE_LIMIT < angle) && (angle < ShooterConstants.TURRET_FORWARD_LIMIT)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index 65edc04..9602267 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -120,14 +120,7 @@ public class Shoot extends CommandBase { m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; // deadzone processing - if (AimToCenter.isHardwareDeadzone(m_targetAngle)) { - m_targetAngle = m_targetAngle + 20; - } - - if (AimToCenter.isDigitalDeadzone(m_targetAngle)) { - // this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work - m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true); - } + if (AimToCenter.isDeadzone(m_targetAngle)) {} // initial error updateError(); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 7a0f45e..43057c8 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -57,8 +57,8 @@ public class Turret extends SubsystemBase { SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); setTurretSoftLimits(true); m_boomBoomRotateMotor.setInverted(false);