diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a68376e..1e6cd5b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,7 +62,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.commands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; @@ -316,7 +316,7 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 91b1854..065ec83 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -188,16 +188,16 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); } -// // Shooter Config -// /* Boom Boom Subsystem */ + // Shooter Config + /* Boom Boom Subsystem */ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); -// // turret subsystem + // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { @@ -219,9 +219,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON - shooterFalconRight.setInverted(false); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.configFactoryDefault(); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.setInverted(false); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); // m_shooterFalconRight.configPeakOutputForward(0, @@ -236,24 +236,18 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); -// } -// // /* Turret Subsytem */ - // shooterFalconRight.configStatorCurrentLimit(new - // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers - // out of our ass anymore - // shooterFalconLeft.configSupplyCurrentLimit(new - // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull -// // numbers out of our ass anymore + // turret + shooterTurret.restoreFactoryDefaults(); + shooterTurret.setIdleMode(IdleMode.kBrake); + shooterTurret.setInverted(true); -// // hood subsystem + // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); } - - - + /* Serializer Subsystem */ public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); @@ -277,14 +271,12 @@ public class RobotMap { extenderMotor.setIdleMode(IdleMode.kBrake); } - void configureSerializerMotors() { - serializerBelt.restoreFactoryDefaults(); - } + void configureSerializerMotors() { + serializerBelt.restoreFactoryDefaults(); + } - /* Storage Subsystem */ + /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); -// public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); -// public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); void configureStorageMotors() { storageMotor.restoreFactoryDefaults(); diff --git a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java similarity index 96% rename from src/main/java/frc4388/robot/commands/RunMiddleSwitch.java rename to src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java index 3a61d55..095cc9f 100644 --- a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.ButtonBoxCommands; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 7eb3572..02ce30a 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -13,13 +13,14 @@ import frc4388.robot.subsystems.Intake; // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class ExtenderIntakeGroup extends ParallelRaceGroup { - public static int direction = 1; // assume extender starts retracted completely + public static int direction; /** Creates a new RunExtenderAndIntake. */ public ExtenderIntakeGroup(Intake intake, Extender extender) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - ExtenderIntakeGroup.direction = 1; // Does this make sense? It kind of defeats the purpose of making it static, does it work without? + + ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5f60cdb..ee63473 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -65,7 +65,7 @@ public class SwerveDrive extends SubsystemBase { m_rightBack = rightBack; m_gyro = gyro; - modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; + modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 403b8c4..7698464 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -29,28 +29,20 @@ public class Turret extends SubsystemBase { public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, // MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - // SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; - public Gyro m_turretGyro; + SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; - public double m_targetDistance = 0; + SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; - public boolean m_isAimReady = false; - - SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); - public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); - - // Variables - public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit.enableLimitSwitch(true); - // m_boomBoomLeftLimit.enableLimitSwitch(true); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + setTurretLimitSwitches(false); // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); @@ -58,8 +50,13 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); setTurretSoftLimits(true); - m_boomBoomRotateMotor.setInverted(true); - + setTurretPIDGains(); + } + + /** + * Set gains for turret PIDs. + */ + public void setTurretPIDGains() { m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -84,6 +81,15 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } + /** + * Set status of turret limit switches. + * @param set Boolean to set limit switches to. + */ + public void setTurretLimitSwitches(boolean set) { + m_boomBoomRightLimit.enableLimitSwitch(set); + m_boomBoomLeftLimit.enableLimitSwitch(set); + } + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; @@ -109,6 +115,13 @@ public class Turret extends SubsystemBase { runShooterRotatePID(0); } + /** + * Run a PID to go to the midpoint position, between the two soft limits. + */ + public void gotoMidpoint() { + runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + public double getboomBoomRotatePosition() { return m_boomBoomRotateEncoder.getPosition(); } @@ -116,7 +129,7 @@ public class Turret extends SubsystemBase { public double getBoomBoomAngleDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; - } + } // TODO: does this method work? public double getCurrent(){ return m_boomBoomRotateMotor.getOutputCurrent(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 748bcd5..08d3d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -86,10 +86,10 @@ public void track(){ public void checkFinished(){ if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ - m_turret.m_isAimReady = true; + // m_turret.m_isAimReady = true; } else{ - m_turret.m_isAimReady = false; + // m_turret.m_isAimReady = false; } }