diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e79e046..0c4c73e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,11 +22,20 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public class BlinkinConstants { + + public static final int kBlinkinPort = 0; + + } public static final class DriveConstants { // TODO turn field oriented on or off public static final boolean kFieldOriented = true; + + //Turn theta + public static final double kTurnThetaMaxSpeed = 0.9; + public static final double kTurnThetaShutoffSensitivity = 0.005; // Distance between centers of right and left wheels on robot public static final double kTrackWidth = Units.inchesToMeters(24.75); @@ -110,7 +119,7 @@ public static final class DriveConstants { public static final double kTeleMaxMetersPerSec = 0.3; public static final double kFastTeleMaxMetersPerSec = 1.0; public static final double kFasterTeleMaxMetersPerSec = 1.8; - public static final double kNudgeSpeed = 0.5; + public static final double kNudgeSpeed = 0.8; public static final Button kTestMotorButton = Button.kLeftBumper; } @@ -144,18 +153,20 @@ public static final class ShooterConstants { public static final int kIndexMotorCanId = 53; public static final int kWinchMotorCanId = 54; + public static final double kShooterMaxSpeed = 1.0; public static final double kFrontShootPower = 1.0; public static final double kBackShootPower = -1.0; public static final double kFrontIntakePower = -0.35; - public static final double kBackIntakePower = -0.30; - public static final double kIndexIntakePower = 0.1; - public static final double kIndexPower = -1.0; + public static final double kBackIntakePower = 0.35; + } + + public static final class WinchySquinchyConstants{ public static final double kWinchUpPower = 0.5; public static final double kWinchDownPower = -1.0; - + public static final double kWinchDeadBand = 0.05; public static final double kWinchUpPreset = 0; - public static final double kWinchDownPreset = 1.5; + public static final double kWinchDownPreset = 1.75;//was 1.5 } public static final class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 320962d..4e05cab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.WinchySquinchyConstants; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.WinchCommand; import frc.robot.commands.WinchPresetCommand; @@ -25,10 +26,12 @@ import frc.robot.commands.auto.ShootAndDriveAutoCommand; import frc.robot.commands.auto.ShootAutoCommand; import frc.robot.commands.auto.TurnAndShootAutoCommand; +import frc.robot.commands.auto.TwoRingAutoCommand; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.WinchySubsystem; import frc.robot.commands.AirIntakeCommand; import frc.robot.commands.ClimbCommand; import frc.robot.commands.ExpelRingCommand; @@ -38,6 +41,7 @@ import frc.robot.commands.ResetWinchCommand; import frc.robot.commands.RetractIntakeCommand; import frc.robot.commands.ShootCommand; +import frc.robot.commands.ShooterCommand; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -51,10 +55,12 @@ public class RobotContainer { // The driver's controller XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); - XboxController m_auxController = new XboxController(OIConstants.kSecondControllerPort); + XboxController + m_auxController = new XboxController(OIConstants.kSecondControllerPort); // Subsystems private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final WinchySubsystem m_winchySubsystem = new WinchySubsystem(); private final SwerveSubsystem m_robotDrive = new SwerveSubsystem(); private boolean fastMode = false; private boolean fasterMode = false; @@ -80,30 +86,32 @@ public RobotContainer() { this::getFasterMode, this::getPOV)); m_climberSubsystem.setDefaultCommand(new ClimbCommand(m_climberSubsystem, this::getAuxRightY, this::getAuxLeftY)); - m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed)); + m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_winchySubsystem, this::POVToWinchSpeed)); m_intakeSubsystem.setDefaultCommand(new RetractIntakeCommand(m_intakeSubsystem)); - // Auto chooser setup + // Auto chooser setup (sqeak!) m_tab = Shuffleboard.getTab("Auto"); m_delay = m_tab.add("Delay", 0).getEntry(); m_chosenAuto.setDefaultOption("Shoot and drive from middle", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //used to be an empty Pose2D (should not change but maybe the empty pose has something to do with it) + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay)); m_chosenAuto.addOption("Shoot and drive from left", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), this::getDelay)); m_chosenAuto.addOption("Shoot and drive from right", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), this::getDelay)); m_chosenAuto.addOption("Drive, turn, and shoot from left", - new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 55, m_delay.getDouble(0))); + new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), 55, this::getDelay)); m_chosenAuto.addOption("Drive, turn, and shoot from right", - new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0))); + new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), -55, this::getDelay)); m_chosenAuto.addOption("Drive back only", - new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto + new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), this::getDelay)); m_chosenAuto.addOption("Shoot only", - new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0)).withTimeout(4)); + new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay).withTimeout(4)); + m_chosenAuto.addOption("2 rings", + new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay)); - m_tab.add(m_chosenAuto); + m_tab.add("Auto", m_chosenAuto); // Configure the button bindings configureButtonBindings(); @@ -126,16 +134,13 @@ public void robotInit() { private void configureButtonBindings() { // Driver stuff - new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) // Reset gyro + new JoystickButton(m_driverController, XboxController.Button.kStart.value) // Reset gyro .whileTrue(new ZeroHeadingCommand(m_robotDrive)); - new JoystickButton(m_driverController, XboxController.Button.kA.value) + new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) .whileTrue(new GroundIntakeCommand(m_intakeSubsystem)); new JoystickButton(m_driverController, XboxController.Button.kB.value) .whileTrue(new ExpelRingCommand(m_intakeSubsystem)); - new JoystickButton(m_driverController, XboxController.Button.kX.value) - .whileTrue(new FeedShooterCommand(m_intakeSubsystem)); - new JoystickButton(m_driverController, XboxController.Button.kY.value) - .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); + // Left bumper = Toggle fastmode // Left trigger = Toggle fastermode // POV = Nudge @@ -144,19 +149,25 @@ private void configureButtonBindings() { // Auxillary stuff new JoystickButton(m_auxController, XboxController.Button.kRightBumper.value) // Shoot - .whileTrue(new ShootCommand(m_shooterSubsystem)); + .whileTrue(new ShootCommand(m_shooterSubsystem, m_intakeSubsystem)); new JoystickButton(m_auxController, XboxController.Button.kLeftBumper.value) // Intake - .whileTrue(new IntakeCommand(m_shooterSubsystem)); - new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up - .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); - new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down - .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); + .whileTrue(new IntakeCommand(m_shooterSubsystem)) + .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); + // new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up + // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); + // new JoystickButton(m_auxControllser, XboxController.Button.kA.value) // Climber down + // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); new JoystickButton(m_auxController, XboxController.Button.kY.value) // Winch up preset - .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchUpPreset)); + .whileTrue(new WinchPresetCommand(m_winchySubsystem, WinchySquinchyConstants.kWinchUpPreset)); new JoystickButton(m_auxController, XboxController.Button.kX.value) // Winch down preset - .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchDownPreset)); + .whileTrue(new WinchPresetCommand(m_winchySubsystem, WinchySquinchyConstants.kWinchDownPreset)); new JoystickButton(m_auxController, XboxController.Button.kBack.value) // Reset winch encoder - .whileTrue(new ResetWinchCommand(m_shooterSubsystem)); + .whileTrue(new ResetWinchCommand(m_winchySubsystem)); + + new JoystickButton(m_auxController, XboxController.Button.kA.value) + .whileTrue(new FeedShooterCommand(m_intakeSubsystem)); + new JoystickButton(m_auxController, XboxController.Button.kB.value) + .whileTrue(new ShooterCommand(m_shooterSubsystem)); // POV = Winch // Joysticks = Manual climbers } @@ -175,6 +186,11 @@ boolean getFasterMode() { else fasterMode = false; return fasterMode; } + + double getDelay() { + return m_delay.getDouble(0); + } + double getRightX() {return m_driverController.getRightX();} double getLeftX() {return -m_driverController.getLeftX();} double getLeftY() {return -m_driverController.getLeftY();} @@ -183,7 +199,7 @@ boolean getFasterMode() { double getAuxLeftY() {return Math.abs(m_auxController.getLeftY()) > OIConstants.kDriveDeadband ? m_auxController.getLeftY() : 0;} double getAuxPOV() {return m_auxController.getPOV();} double POVToWinchSpeed() { - return getAuxPOV() == 0 ? ShooterConstants.kWinchUpPower : (getAuxPOV() == 180 ? ShooterConstants.kWinchDownPower : 0); + return getAuxPOV() == 0 ? WinchySquinchyConstants.kWinchUpPower : (getAuxPOV() == 180 ? WinchySquinchyConstants.kWinchDownPower : 0); } /** diff --git a/src/main/java/frc/robot/commands/GroundIntakeCommand.java b/src/main/java/frc/robot/commands/GroundIntakeCommand.java index 6b21c59..55171a0 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeCommand.java @@ -4,6 +4,9 @@ package frc.robot.commands; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; @@ -30,6 +33,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_subsystem.m_intakeLimitSwitch.isPressed(); + // return false; } } diff --git a/src/main/java/frc/robot/commands/ResetWinchCommand.java b/src/main/java/frc/robot/commands/ResetWinchCommand.java index 2322ee6..865842c 100644 --- a/src/main/java/frc/robot/commands/ResetWinchCommand.java +++ b/src/main/java/frc/robot/commands/ResetWinchCommand.java @@ -2,11 +2,12 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.WinchySubsystem; public class ResetWinchCommand extends Command { - private final ShooterSubsystem m_subsystem; + private final WinchySubsystem m_subsystem; - public ResetWinchCommand(ShooterSubsystem subsystem) { + public ResetWinchCommand(WinchySubsystem subsystem) { m_subsystem = subsystem; addRequirements(subsystem); } diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index d66c2da..09c62e8 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -1,14 +1,20 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class ShootCommand extends SequentialCommandGroup { - public ShootCommand ( - ShooterSubsystem shooterSubsystem - ) { + public ShootCommand (ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { addCommands( - new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75), - new ShooterCommand(shooterSubsystem, "all") + new ParallelCommandGroup( + new AirIntakeCommand(intakeSubsystem).withTimeout(0.2), + new ShooterCommand(shooterSubsystem, "both") + ).withTimeout(1.75), + new ParallelCommandGroup( + new ShooterCommand(shooterSubsystem, "both"), + new FeedShooterCommand(intakeSubsystem) + ) ); } } diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index ddfdb4d..62db8fb 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -23,19 +23,15 @@ public ShooterCommand(ShooterSubsystem subsystem) { @Override public void execute() { if (m_commandType.equals("both")) m_subsystem.runShooter(); - else if (m_commandType.equals("all")) { - m_subsystem.runShooter(); - m_subsystem.runIndex(); - } else if (m_commandType.equals("front")) m_subsystem.runShooter(0.0); - else if (m_commandType.equals("index")) m_subsystem.runIndex(); + // else if (m_commandType.equals("index")) m_subsystem.runIndex(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { if (m_commandType.equals("both") || m_commandType.equals("front") || m_commandType.equals("all")) m_subsystem.stopShooter(); - if (m_commandType.equals("index") || m_commandType.equals("all")) m_subsystem.stopIndex(); + // if (m_commandType.equals("index") || m_commandType.equals("all")) m_subsystem.stopIndex(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/TurnByThetaCommand.java b/src/main/java/frc/robot/commands/TurnByThetaCommand.java new file mode 100644 index 0000000..ec7c8f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurnByThetaCommand.java @@ -0,0 +1,142 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.OIConstants; +import frc.robot.subsystems.SwerveSubsystem; +import java.util.function.Supplier; + +public class TurnByThetaCommand extends Command { + + public final SwerveSubsystem swerveSubsystem; + public final Supplier xSpdFunction, ySpdFunction, turningSpdFunction; + public final Supplier fieldOrientedFunction, fastModeFunction; + public final SlewRateLimiter xLimiter, yLimiter, turningLimiter; + public final Supplier fieldOriented = ()-> false; + public final Supplier fastMode = ()-> false; + public final Supplier xSupplier = ()-> 0.0; + public final Supplier ySupplier = ()-> 0.0; + public final Supplier turnSupplier = ()-> 0.0; + private boolean isTurnFinished = false; + private double targetTheta, initialHeading; + + public TurnByThetaCommand(SwerveSubsystem subsystem, double turnDegrees) { + swerveSubsystem = subsystem; + xSpdFunction = xSupplier; + ySpdFunction = ySupplier; + turningSpdFunction = turnSupplier; + fieldOrientedFunction = fieldOriented; + fastModeFunction = fastMode; + targetTheta = turnDegrees; + xLimiter = new SlewRateLimiter(DriveConstants.kXSlewRateLimit); + yLimiter = new SlewRateLimiter(DriveConstants.kYSlewRateLimit); + turningLimiter = new SlewRateLimiter(DriveConstants.kTurnSlewRateLimit); + addRequirements(swerveSubsystem); + } + + @Override + public void initialize(){ + swerveSubsystem.resetPose(); + isTurnFinished = false; + initialHeading = swerveSubsystem.getHeading(); + if(initialHeading < 0){ + initialHeading = initialHeading + 180 + 360; + } + } + + @Override + public void execute() { + //autoturny stuffs + targetTheta += initialHeading; + double currentHeading = swerveSubsystem.getHeading(); + double currentRawHeading = swerveSubsystem.getHeading(); + double theta,speed; + if(currentHeading < 0){ + currentHeading = currentHeading + 180 + 360; + } + currentHeading %= 360; + theta = targetTheta - currentHeading; + speed = theta / 45; + + if(targetTheta < 0 && initialHeading >= 0 && initialHeading < 180 + targetTheta){ + if(currentHeading < 10){ + theta -= .1; //keep robot moving over the boundery + } + if(currentHeading < 0 || currentHeading > 180 + targetTheta){ + theta = (targetTheta + 360) - currentHeading; //robot move in correct direction once over boundery + } + }else if(targetTheta > 0 && initialHeading <= 0 && initialHeading > 180 + targetTheta){ + if(currentHeading > 350){ + theta += .1; //keep robot moving over the boundery + } + if(currentHeading > 359 || currentHeading < targetTheta){ + theta = (360 - targetTheta) - currentHeading;//robot move in correct direction once over boundery + } + } + System.out.println("DeltaTheta:" + theta); + System.out.println("TargetTheta:" + targetTheta); + System.out.println("Current Heading:" + currentHeading); + + + + + // Get inputs + double xSpeed = xSpdFunction.get(); + double ySpeed = ySpdFunction.get(); + double turningSpeed = turningSpdFunction.get(); + boolean fastMode = fastModeFunction.get(); + + //Hyjack joysticks + turningSpeed = MathUtil.clamp(speed, -DriveConstants.kTurnThetaMaxSpeed, DriveConstants.kTurnThetaMaxSpeed); //You's we;come constant police + xSpeed = 0; + ySpeed = 0; + System.out.println("TurningSpeed:" + turningSpeed); + + // Death + xSpeed = Math.abs(xSpeed) > OIConstants.kDriveDeadband ? xSpeed : 0; + ySpeed = Math.abs(ySpeed) > OIConstants.kDriveDeadband ? ySpeed : 0; + turningSpeed = Math.abs(turningSpeed) > OIConstants.kDriveDeadband ? turningSpeed : 0; + + //Actual death + if(Math.abs(turningSpeed) < DriveConstants.kTurnThetaShutoffSensitivity){ //TODO may need to adjust how sensitive it is + isTurnFinished = true; + } + + // Slew soup + double maxDriveSpeed = fastMode ? DriveConstants.kFastTeleMaxMetersPerSec : DriveConstants.kTeleMaxMetersPerSec; + double maxTurnSpeed = fastMode ? DriveConstants.kFastTeleMaxRadiansPerSec : DriveConstants.kTeleMaxRadiansPerSec; + xSpeed = xLimiter.calculate(xSpeed) * maxDriveSpeed; + ySpeed = yLimiter.calculate(ySpeed) * maxDriveSpeed; + turningSpeed = turningLimiter.calculate(turningSpeed) * maxTurnSpeed; + + // I am speed + ChassisSpeeds chassisSpeeds; + if (fieldOrientedFunction.get()) { + // Field oriented + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d()); + } + else { + chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed); + } + + // Divide and conker + SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + + // Actually do the thing + swerveSubsystem.setModuleStates(moduleStates); + } + + @Override + public void end(boolean interrupted) { + swerveSubsystem.stopModules(); + } + + @Override + public boolean isFinished() { + return isTurnFinished; + } +} diff --git a/src/main/java/frc/robot/commands/TurnThetaCommand.java b/src/main/java/frc/robot/commands/TurnThetaCommand.java index 3308af5..62e3667 100644 --- a/src/main/java/frc/robot/commands/TurnThetaCommand.java +++ b/src/main/java/frc/robot/commands/TurnThetaCommand.java @@ -22,7 +22,6 @@ public class TurnThetaCommand extends Command { public final Supplier ySupplier = ()-> 0.0; public final Supplier turnSupplier = ()-> 0.0; private boolean isTurnFinished = false; - private double initial_heading; private double targetTheta; public TurnThetaCommand(SwerveSubsystem subsystem, double turnDegrees) { @@ -42,7 +41,6 @@ public TurnThetaCommand(SwerveSubsystem subsystem, double turnDegrees) { @Override public void initialize(){ swerveSubsystem.resetPose(); - initial_heading = swerveSubsystem.getHeading(); isTurnFinished = false; } diff --git a/src/main/java/frc/robot/commands/WinchCommand.java b/src/main/java/frc/robot/commands/WinchCommand.java index 5f96b82..440f813 100644 --- a/src/main/java/frc/robot/commands/WinchCommand.java +++ b/src/main/java/frc/robot/commands/WinchCommand.java @@ -4,12 +4,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.WinchySubsystem; public class WinchCommand extends Command { - private final ShooterSubsystem m_subsystem; + private final WinchySubsystem m_subsystem; private final Supplier m_power; - public WinchCommand(ShooterSubsystem subsystem, Supplier power) { + public WinchCommand(WinchySubsystem subsystem, Supplier power) { m_subsystem = subsystem; m_power = power; addRequirements(subsystem); diff --git a/src/main/java/frc/robot/commands/WinchPresetCommand.java b/src/main/java/frc/robot/commands/WinchPresetCommand.java index a62eb7e..66cbb16 100644 --- a/src/main/java/frc/robot/commands/WinchPresetCommand.java +++ b/src/main/java/frc/robot/commands/WinchPresetCommand.java @@ -4,19 +4,21 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.WinchySquinchyConstants; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.WinchySubsystem; public class WinchPresetCommand extends Command { - private final ShooterSubsystem m_subsystem; + private final WinchySubsystem m_subsystem; private final Supplier m_goal; - public WinchPresetCommand(ShooterSubsystem subsystem, Supplier goalPosition) { + public WinchPresetCommand(WinchySubsystem subsystem, Supplier goalPosition) { m_subsystem = subsystem; m_goal = goalPosition; addRequirements(subsystem); } - public WinchPresetCommand(ShooterSubsystem subsystem, double goalPosition) { + public WinchPresetCommand(WinchySubsystem subsystem, double goalPosition) { m_subsystem = subsystem; m_goal = () -> goalPosition; addRequirements(subsystem); @@ -42,6 +44,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Math.abs(m_subsystem.getWinchEncoder() - m_goal.get()) < ShooterConstants.kWinchDeadBand; + return Math.abs(m_subsystem.getWinchEncoder() - m_goal.get()) < WinchySquinchyConstants.kWinchDeadBand; } } diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java index da73aff..c81208d 100644 --- a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java @@ -7,24 +7,21 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; -import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.TurnThetaCommand; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class DriveAutoCommand extends SequentialCommandGroup { - public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) { + public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) - .withTimeout(3), + .withTimeout(7), new TurnThetaCommand(swerve, 0) ); } diff --git a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java index e5570cb..01a77db 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java @@ -7,22 +7,21 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.TurnThetaCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class ShootAndDriveAutoCommand extends SequentialCommandGroup { - public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) { + public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), - new ShootCommand(shoot).withTimeout(4), + new WaitCommand(delay.get()), + new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(3), diff --git a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java index af9b6d7..1eddd1b 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java @@ -4,21 +4,22 @@ package frc.robot.commands.auto; +import java.util.function.Supplier; + import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class ShootAutoCommand extends SequentialCommandGroup { - public ShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) { + public ShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), - new ShootCommand(shoot).withTimeout(4), + new WaitCommand(delay.get()), + new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0) ); } diff --git a/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java index cb3b664..047502d 100644 --- a/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java @@ -7,24 +7,23 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class SlideAndShootAutoCommand extends SequentialCommandGroup { - public SlideAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, int direction) { + public SlideAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, int direction) { super( new WaitCommand(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(direction == 0 ? -.3 : .3), supplyDouble(0), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(0)) .withTimeout(1), - new ShootCommand(shoot), + new ShootCommand(shoot, intake), new SwerveDriveCommand(swerve, supplyDouble(direction == 0 ? .3 : -.3), supplyDouble(0), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(0)) .withTimeout(1), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(0)) diff --git a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java index 374938b..a29c944 100644 --- a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java @@ -7,26 +7,25 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.TurnThetaCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class TurnAndShootAutoCommand extends SequentialCommandGroup { - public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double turnDegrees, double delay) { + public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double turnDegrees, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(1.9), new TurnThetaCommand(swerve, turnDegrees).withTimeout(1), - new ShootCommand(shoot).withTimeout(4), + new ShootCommand(shoot, intake).withTimeout(4), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(2.8) ); diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java new file mode 100644 index 0000000..8044757 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// 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 frc.robot.commands.auto; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.GroundIntakeCommand; +import frc.robot.commands.RetractIntakeCommand; +import frc.robot.commands.SetPoseCommand; +import frc.robot.commands.ShootCommand; +import frc.robot.commands.SwerveDriveCommand; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +public class TwoRingAutoCommand extends SequentialCommandGroup { + public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, Supplier delay) { + super( + new WaitCommand(delay.get()), + new ShootCommand(shoot, intake).withTimeout(4), + new SetPoseCommand(swerve, startingOffset).withTimeout(0), + new ParallelCommandGroup( + new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), + new GroundIntakeCommand(intake) + ).withTimeout(3), + new ParallelCommandGroup( + new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), + new RetractIntakeCommand(intake) + ) + .withTimeout(3.2), + new ShootCommand(shoot, intake).withTimeout(4) + ); + } + + private static Supplier supplyDouble(double d) {return new Supplier() {@Override public Double get() {return d;}};} + private static Supplier supplyBoolean(boolean b) {return new Supplier() {@Override public Boolean get() {return b;}};} +} diff --git a/src/main/java/frc/robot/subsystems/BlinkinSubsystem.java b/src/main/java/frc/robot/subsystems/BlinkinSubsystem.java new file mode 100644 index 0000000..19d444c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/BlinkinSubsystem.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// 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 frc.robot.subsystems; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.BlinkinConstants; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + + +public class BlinkinSubsystem extends SubsystemBase { + + private final ShuffleboardTab m_tab = Shuffleboard.getTab("Blinkin"); + + private final Spark blinkin; + + public BlinkinSubsystem() { + blinkin = new Spark(BlinkinConstants.kBlinkinPort); + m_tab.add(blinkin.toString(), blinkin); + } + + public BooleanSupplier supplier = () -> {return false;}; + + @Override + public void periodic() { + if (supplier.getAsBoolean()) + setGreen(); + // This method will be called once per scheduler run + } + + public void set(double pattern) { + blinkin.set(pattern); + } + + public void setOcean(){ + blinkin.set(-0.95); + } + + public void setRainbow(){ + blinkin.set(-0.99); + } + + public void setBlue(){ + blinkin.set(0.87); + } + + public void setGreen(){ + blinkin.set(0.77); + } + + public void setRed(){ + blinkin.set(0.61); + } + + public void setWhite(){ + blinkin.set(0.93); + } + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1ad78ad..b3993e4 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -5,6 +5,7 @@ import com.revrobotics.SparkRelativeEncoder.Type; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkLimitSwitch; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -18,6 +19,8 @@ public class IntakeSubsystem extends SubsystemBase { public final CANSparkMax m_intakeMotor; public final RelativeEncoder m_flippyEncoder; + public final SparkLimitSwitch m_intakeLimitSwitch; + public final ShuffleboardTab m_tab; public IntakeSubsystem() { @@ -30,6 +33,8 @@ public IntakeSubsystem() { m_flippyMotor.setIdleMode(IdleMode.kCoast); m_intakeMotor.setIdleMode(IdleMode.kBrake); + m_intakeLimitSwitch = m_intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + m_tab = Shuffleboard.getTab("Intake"); m_tab.addNumber("Intake Current", this::getIntakeCurrent); m_tab.addNumber("Flipper Position", this::getFlipperEncoder); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 16c6734..5dca565 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -2,9 +2,11 @@ // NEGATIVE CLOCKWISE package frc.robot.subsystems; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkRelativeEncoder.Type; @@ -21,36 +23,27 @@ public class ShooterSubsystem extends SubsystemBase { private final ShuffleboardTab m_shuffleboardtab = Shuffleboard.getTab("Shooter"); - private final GenericEntry m_frontMotorPower; - private final GenericEntry m_backMotorPower; - private final GenericEntry m_frontIntakePower; - private final GenericEntry m_backIntakePower; - private final GenericEntry m_indexPower; - private final GenericEntry m_indexIntakePower; - private final GenericEntry m_winchUpPower; - private final GenericEntry m_winchDownPower; + private final GenericEntry m_frontMotorPower, m_backMotorPower, m_frontIntakePower, m_backIntakePower; + + public final CANSparkFlex m_frontMotor, m_backMotor; + public final SparkPIDController m_frontPID, m_backPID; - public final CANSparkFlex m_frontMotor; - public final CANSparkFlex m_backMotor; - public final CANSparkMax m_indexMotor; - public final CANSparkMax m_winchMotor; - public final RelativeEncoder m_winchEncoder; public ShooterSubsystem() { m_frontMotor = new CANSparkFlex(ShooterConstants.kFrontMotorCanId, MotorType.kBrushless); m_backMotor = new CANSparkFlex(ShooterConstants.kBackMotorCanId, MotorType.kBrushless); - m_indexMotor = new CANSparkMax(ShooterConstants.kIndexMotorCanId, MotorType.kBrushless); - m_winchMotor = new CANSparkMax(ShooterConstants.kWinchMotorCanId, MotorType.kBrushed); + m_frontMotor.setInverted(false); + m_backMotor.setInverted(false); + m_frontPID = m_frontMotor.getPIDController(); + m_backPID = m_backMotor.getPIDController(); + m_frontPID.setReference(0, CANSparkBase.ControlType.kVelocity); + m_backPID.setReference(0, CANSparkBase.ControlType.kVelocity); m_frontMotor.setIdleMode(IdleMode.kCoast); m_backMotor.setIdleMode(IdleMode.kCoast); - m_indexMotor.setIdleMode(IdleMode.kCoast); - m_winchMotor.setIdleMode(IdleMode.kCoast); m_frontMotor.setInverted(false); m_backMotor.setInverted(false); - m_indexMotor.setInverted(true); - m_winchMotor.setInverted(false); - m_winchEncoder = m_winchMotor.getEncoder(Type.kQuadrature, 8192); + // Displays whether or not the shooter is running m_shuffleboardtab.addBoolean("Shooter Running", this::isShooterRunning); @@ -64,31 +57,24 @@ public ShooterSubsystem() { .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_backIntakePower = m_shuffleboardtab.add("Back Intake Power", ShooterConstants.kBackIntakePower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_indexPower = m_shuffleboardtab.add("Index Power", ShooterConstants.kIndexPower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_indexIntakePower = m_shuffleboardtab.add("Index Intake Power", ShooterConstants.kIndexIntakePower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_winchUpPower = m_shuffleboardtab.add("Winch Up Power", ShooterConstants.kWinchUpPower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", ShooterConstants.kWinchDownPower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - // Winch encoder value - m_shuffleboardtab.addNumber("Winch Encoder", this::getWinchEncoder); + m_shuffleboardtab.addBoolean("Shooter spun up", this::getSpunUp); + m_shuffleboardtab.addNumber("Front motor RPM", this::getFrontRPM); + m_shuffleboardtab.addNumber("Back motor RPM", this::getBackRPM); } // Shooter stuff public double[] getShooterPowers() { // Function to get the motor powers from shuffleboard and clamp them to a value between -1 and 1 return new double[] { - clampPower(m_frontMotorPower.getDouble(ShooterConstants.kFrontShootPower)), - clampPower(m_backMotorPower.getDouble(ShooterConstants.kBackShootPower)) + clampPower(m_frontMotorPower.getDouble(ShooterConstants.kFrontShootPower)*ShooterConstants.kShooterMaxSpeed), + clampPower(m_backMotorPower.getDouble(ShooterConstants.kBackShootPower)*ShooterConstants.kShooterMaxSpeed) }; } public double[] getIntakePowers() { return new double[] { - clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)), - clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)), - clampPower(m_indexIntakePower.getDouble(ShooterConstants.kIndexIntakePower)) + clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)*ShooterConstants.kShooterMaxSpeed), + clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)*ShooterConstants.kShooterMaxSpeed) + // clampPower(m_indexIntakePower.getDouble(ShooterConstants.kIndexIntakePower)) }; } public void runShooter() { @@ -97,17 +83,17 @@ public void runShooter() { } public void runShooter(double rearPower) { m_frontMotor.set(getShooterPowers()[0]); - m_backMotor.set(rearPower); + m_backMotor.set(rearPower*ShooterConstants.kShooterMaxSpeed); } public void runIntake() { m_frontMotor.set(getIntakePowers()[0]); m_backMotor.set(getIntakePowers()[1]); - m_indexMotor.set(getIntakePowers()[2]); + // m_indexMotor.set(getIntakePowers()[2]); } public void stopShooter() { m_frontMotor.stopMotor(); m_backMotor.stopMotor(); - m_indexMotor.stopMotor(); + // m_indexMotor.stopMotor(); } public boolean isShooterRunning() { if (m_frontMotor.get() == 0 && m_backMotor.get() == 0) { //Check if both motors are stopped @@ -116,25 +102,16 @@ public boolean isShooterRunning() { return true; //Otherwise, the shooter is running } } - - // Indexer stuff - public double getIndexPower() {return clampPower(m_indexPower.getDouble(ShooterConstants.kIndexPower));} - public void runIndex() {m_indexMotor.set(getIndexPower());} - public void stopIndex() {m_indexMotor.stopMotor();} - - // Winch stuff - public double[] getWinchPowers() { - return new double[] { - clampPower(m_winchUpPower.getDouble(ShooterConstants.kWinchUpPower)), - clampPower(m_winchDownPower.getDouble(ShooterConstants.kWinchDownPower)) - }; + public double getFrontRPM() { + return m_frontMotor.getEncoder().getVelocity(); + } + public double getBackRPM() { + return m_backMotor.getEncoder().getVelocity(); + } + public boolean getSpunUp() { + return Math.abs(m_frontMotor.getEncoder().getVelocity()) >= 6000 + && Math.abs(m_backMotor.getEncoder().getVelocity()) >= 6000; } - public void winchUp() {m_winchMotor.set(getWinchPowers()[0]);} - public void winchDown() {m_winchMotor.set(getWinchPowers()[1]);} - public void stopWinch() {m_winchMotor.stopMotor();} - public void manualWinch(double power) {m_winchMotor.set(power);} - public double getWinchEncoder() {return m_winchEncoder.getPosition();} - public void resetWinchEncoder() {m_winchEncoder.setPosition(0);} // Function to clamp the power to a value between -1 and 1 public static double clampPower(double power) { diff --git a/src/main/java/frc/robot/subsystems/WinchySubsystem.java b/src/main/java/frc/robot/subsystems/WinchySubsystem.java new file mode 100644 index 0000000..24ac4a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/WinchySubsystem.java @@ -0,0 +1,67 @@ +// POSITIVE COUNTERCLOCKWISE +// NEGATIVE CLOCKWISE +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkRelativeEncoder.Type; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.WinchySquinchyConstants; + +import java.util.Map; + +public class WinchySubsystem extends SubsystemBase { + + private final ShuffleboardTab m_shuffleboardtab = Shuffleboard.getTab("Winchy"); + + private final GenericEntry m_winchUpPower, m_winchDownPower; + + public final CANSparkMax m_winchMotor; + public final RelativeEncoder m_winchEncoder; + + public WinchySubsystem() { + m_winchMotor = new CANSparkMax(ShooterConstants.kWinchMotorCanId, MotorType.kBrushed); + m_winchMotor.setIdleMode(IdleMode.kBrake); + m_winchMotor.setInverted(false); + m_winchEncoder = m_winchMotor.getEncoder(Type.kQuadrature, 8192); + + // Shuffleboard numbers for the winchy squinchy + m_winchUpPower = m_shuffleboardtab.add("Winch Up Power", WinchySquinchyConstants.kWinchUpPower) + .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", WinchySquinchyConstants.kWinchDownPower) + .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + + // Winch encoder value + m_shuffleboardtab.addNumber("Winch Encoder", this::getWinchEncoder); + } + + // Winch stuff + public double[] getWinchPowers() { + return new double[] { + clampPower(m_winchUpPower.getDouble(WinchySquinchyConstants.kWinchUpPower)), + clampPower(m_winchDownPower.getDouble(WinchySquinchyConstants.kWinchDownPower)) + }; + } + public void winchUp() {m_winchMotor.set(getWinchPowers()[0]);} + public void winchDown() {m_winchMotor.set(getWinchPowers()[1]);} + public void stopWinch() {m_winchMotor.stopMotor();} + public void manualWinch(double power) {m_winchMotor.set(power);} + public double getWinchEncoder() {return m_winchEncoder.getPosition();} + public void resetWinchEncoder() {m_winchEncoder.setPosition(0);} + + // Function to clamp the power to a value between -1 and 1 + public static double clampPower(double power) { + return MathUtil.clamp(power, -1, 1); + } +}