diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8d7984a..0434331 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,7 +94,6 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; public static final int SHOOTER_ROTATE_ID = 31; // TODO: find - public static final int TURRET_RIGHT_SOFT_LIMIT = 0; public static final double TURRET_SPEED_MULTIPLIER = 0.1d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; @@ -126,6 +125,9 @@ public final class Constants { 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 TURRET_FORWARD_LIMIT = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find + } public static final class VisionConstants { public static final double TURN_P_VALUE = 0.8; diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 21e7a73..a4eca33 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -6,6 +6,10 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import java.util.concurrent.TimeoutException; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -16,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.Shoot; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -52,12 +57,10 @@ 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.enableSoftLimit(SoftLimitDirection.kForward, false); - // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - // m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set - // second - // soft - // limit + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); + setSoftLimits(true); + m_boomBoomRotateMotor.setInverted(false); m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); @@ -73,6 +76,15 @@ public class Turret extends SubsystemBase { // This method will be called once per scheduler run } + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1;