diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0434331..59cab07 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -108,6 +108,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 = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find // deadzones public static final double HARD_DEADZONE_LEFT = 0.0; @@ -120,13 +122,14 @@ public final class Constants { public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values + /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 32; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find + public static final float HOOD_FORWARD_LIMIT = 0; //TODO: find + public static final float HOOD_REVERSE_LIMIT = 0; //TODO: find - public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find - public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find } public static final class VisionConstants { diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 995af74..b3a2f30 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,10 +39,14 @@ public double m_fireAngle; public Hood() { m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodUpLimitSwitch.enableLimitSwitch(true); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); m_hoodDownLimitSwitch.enableLimitSwitch(true); + + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + setHoodSoftLimits(true); } @@ -49,6 +54,16 @@ public double m_fireAngle; public void periodic() { // This method will be called once per scheduler run } + + /** + * Set status of hood motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setHoodSoftLimits(boolean set) { + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index a4eca33..3a00f02 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -59,7 +59,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); - setSoftLimits(true); + setTurretSoftLimits(true); m_boomBoomRotateMotor.setInverted(false); @@ -80,7 +80,7 @@ public class Turret extends SubsystemBase { * Set status of turret motor soft limits. * @param set Boolean to set soft limits to. */ - public void setSoftLimits(boolean set) { + public void setTurretSoftLimits(boolean set) { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); }