diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index cf56b477..1a270763 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -131,11 +131,12 @@ private void bindDrivebaseControls() { private void bindIntakeControls() { // CommandScheduler.getInstance() // .setDefaultCommand(s.intakeSubsystem, new IntakeStopCommand(s.intakeSubsystem)); - driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem)); - // driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); + driveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); + driveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); + driveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); driveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); - codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem)); + codriveIntakeInButton.onTrue(new AllInCommand(s.intakeSubsystem, this)); codriveIntakeStopButton.onTrue(new AllStopCommand(s.intakeSubsystem)); codriveIntakeReverseButton.onTrue(new AllReverseCommand(s.intakeSubsystem)); codriveIntakeRejectButton.onTrue(new IntakeRejectCommand(s.intakeSubsystem)); @@ -211,6 +212,7 @@ private void bindSysIdControls() { public void vibrateDriveController(double vibration) { if (!DriverStation.isAutonomous()) { driveController.getHID().setRumble(RumbleType.kBothRumble, vibration); + codriveController.getHID().setRumble(RumbleType.kBothRumble, vibration); } } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 9e1ef618..9152862f 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -1,13 +1,22 @@ package frc.team2412.robot.commands.intake; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.team2412.robot.Controls; import frc.team2412.robot.subsystems.IntakeSubsystem; public class AllInCommand extends Command { private final IntakeSubsystem intakeSubsystem; + private final Controls controls; + private boolean rumbledIntakeFront = false; + private boolean rumbledIntakeLeft = false; + private boolean rumbledIntakeRight = false; + private boolean rumbledIndex = false; - public AllInCommand(IntakeSubsystem intakeSubsystem) { + public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { this.intakeSubsystem = intakeSubsystem; + this.controls = controls; addRequirements(intakeSubsystem); } @@ -20,16 +29,94 @@ public void initialize() { @Override public void execute() { + // intake rejecting + if (intakeSubsystem.intakeFrontSeesNote()) { + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeLeftReject(); + intakeSubsystem.intakeRightReject(); + } else { + intakeSubsystem.intakeBackStop(); + intakeSubsystem.intakeLeftStop(); + intakeSubsystem.intakeRightStop(); + } + + if (controls != null && !rumbledIntakeFront) { + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeFront = true; + } + } + + // back intake sensor does not exist (but it might?) + + if (intakeSubsystem.intakeLeftSeesNote()) { + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeRightReject(); + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeBackStop(); + intakeSubsystem.intakeRightStop(); + } + + if (controls != null && !rumbledIntakeLeft) { + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeLeft = true; + } + } + + if (intakeSubsystem.intakeRightSeesNote()) { + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeFrontReject(); + intakeSubsystem.intakeBackReject(); + intakeSubsystem.intakeLeftReject(); + } else { + intakeSubsystem.intakeFrontStop(); + intakeSubsystem.intakeBackStop(); + intakeSubsystem.intakeLeftStop(); + } + + if (controls != null && !rumbledIntakeRight) { + Commands.race(new RumbleCommand(controls), new WaitCommand(2)).schedule(); + rumbledIntakeRight = true; + } + } + + // all intake motors rejecting after index if (intakeSubsystem.indexSensorHasNote()) { - intakeSubsystem.intakeReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeReject(); + } else { + intakeSubsystem.intakeStop(); + } + + if (controls != null && !rumbledIndex) { + Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + rumbledIndex = true; + } } } @Override public void end(boolean interrupted) { - // intakeSubsystem.intakeReject(); + if (!intakeSubsystem.getRejectOverride()) { + intakeSubsystem.intakeReject(); + } else { + intakeSubsystem.intakeStop(); + } + intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); + + if (controls != null) { + Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + } + + rumbledIntakeFront = false; + rumbledIntakeLeft = false; + rumbledIntakeRight = false; + rumbledIndex = false; } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java new file mode 100644 index 00000000..17e459e9 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java @@ -0,0 +1,25 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.IntakeSubsystem; + +public class AllInNoSensorCommand extends Command { + private final IntakeSubsystem intakeSubsystem; + + public AllInNoSensorCommand(IntakeSubsystem intakeSubsystem) { + this.intakeSubsystem = intakeSubsystem; + addRequirements(intakeSubsystem); + } + + @Override + public void initialize() { + intakeSubsystem.intakeIn(); + intakeSubsystem.indexIn(); + intakeSubsystem.feederIn(); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java new file mode 100644 index 00000000..4165bb0e --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java @@ -0,0 +1,27 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.Controls; + +public class RumbleCommand extends Command { + private final Controls controls; + + public RumbleCommand(Controls controls) { + this.controls = controls; + } + + @Override + public void initialize() { + controls.vibrateDriveController(1.0); + } + + @Override + public void end(boolean interrupted) { + controls.vibrateDriveController(0.0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index 7634d431..e7edeaa6 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -1,5 +1,7 @@ package frc.team2412.robot.commands.launcher; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -38,7 +40,9 @@ public FullTargetCommand( this.launcherSubsystem = launcherSubsystem; this.drivebaseSubsystem = drivebaseSubsystem; this.controls = controls; - yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false); + if (DRIVEBASE_ENABLED) { + yawAlignmentCommand = drivebaseSubsystem.rotateToAngle(() -> yawTarget, false); + } addRequirements(launcherSubsystem); } diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 55a14898..6426a8ba 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -7,6 +7,7 @@ import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkLimitSwitch; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -48,6 +49,11 @@ public class IntakeSubsystem extends SubsystemBase { private final DigitalInput indexSensor; private final DigitalInput feederSensor; + private final SparkLimitSwitch intakeFrontSensor; + // private final SparkLimitSwitch intakeBackSensor; + private final SparkLimitSwitch intakeLeftSensor; + private final SparkLimitSwitch intakeRightSensor; + // Shuffleboard private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Intake"); @@ -61,7 +67,11 @@ public class IntakeSubsystem extends SubsystemBase { // sensor override private GenericEntry sensorOverride; + // reject override + private GenericEntry rejectOverride; + public IntakeSubsystem() { + intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); intakeMotorBack = new CANSparkMax(INTAKE_MOTOR_BACK, MotorType.kBrushless); intakeMotorLeft = new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless); @@ -75,6 +85,17 @@ public IntakeSubsystem() { indexSensor = new DigitalInput(INDEX_SENSOR); feederSensor = new DigitalInput(FEEDER_SENSOR); + intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeBackSensor = + // intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + + // todo: MOVE THIS TO CONFIGURE MOTOR + intakeFrontSensor.enableLimitSwitch(false); + intakeLeftSensor.enableLimitSwitch(false); + intakeRightSensor.enableLimitSwitch(false); + resetMotors(); initShuffleboard(); @@ -126,14 +147,48 @@ public void intakeReverse() { intakeSet(INTAKE_REVERSE_SPEED); } + // intake stop methods public void intakeStop() { intakeSet(0); } + public void intakeFrontStop() { + intakeMotorFront.set(0); + } + + public void intakeBackStop() { + intakeMotorBack.set(0); + } + + public void intakeLeftStop() { + intakeMotorLeft.set(0); + } + + public void intakeRightStop() { + intakeMotorRight.set(0); + } + + // intake reject methods public void intakeReject() { intakeSet(INTAKE_REJECT_SPEED); } + public void intakeFrontReject() { + intakeMotorFront.set(INTAKE_REJECT_SPEED); + } + + public void intakeBackReject() { + intakeMotorBack.set(INTAKE_REJECT_SPEED); + } + + public void intakeLeftReject() { + intakeMotorLeft.set(INTAKE_REJECT_SPEED); + } + + public void intakeRightReject() { + intakeMotorRight.set(INTAKE_REJECT_SPEED); + } + // index methods public void indexIn() { indexMotorUpper.set(setIndexInSpeedEntry.getDouble(INDEX_UPPER_IN_SPEED)); @@ -174,12 +229,28 @@ public boolean feederSensorHasNote() { return !feederSensor.get() && !getSensorOverride(); } + public boolean intakeFrontSeesNote() { + return intakeFrontSensor.isPressed(); + } + + public boolean intakeLeftSeesNote() { + return intakeLeftSensor.isPressed(); + } + + public boolean intakeRightSeesNote() { + return intakeRightSensor.isPressed(); + } + + // override methods on shuffleboard public boolean getSensorOverride() { return sensorOverride.getBoolean(false); } - // logging + public boolean getRejectOverride() { + return rejectOverride.getBoolean(false); + } + // logging public void initShuffleboard() { if (Robot.isDebugMode()) { shuffleboardTab @@ -215,6 +286,11 @@ public void initShuffleboard() { shuffleboardTab.addBoolean("Index Sensor - ", this::indexSensorHasNote).withSize(1, 1); shuffleboardTab.addBoolean("Feeder Sensor - ", this::feederSensorHasNote).withSize(1, 1); + // no intake back sensor + shuffleboardTab.addBoolean("Intake Front Sensor - ", this::intakeFrontSeesNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Left Sensor - ", this::intakeLeftSeesNote).withSize(1, 1); + shuffleboardTab.addBoolean("Intake Right Sensor - ", this::intakeRightSeesNote).withSize(1, 1); + setIntakeInSpeedEntry = shuffleboardTab .addPersistent("Intake in speed - ", INTAKE_IN_SPEED) @@ -234,5 +310,12 @@ public void initShuffleboard() { .withSize(1, 1) .withWidget(BuiltInWidgets.kToggleSwitch) .getEntry(); + + rejectOverride = + Shuffleboard.getTab("Intake") + .add("Override Intake Reject", false) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); } } diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 8067393b..8dcd81da 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -52,7 +52,7 @@ public void registerCommands() { // Intake NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem)); + NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); NamedCommands.registerCommand( "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); // Launcher @@ -74,7 +74,6 @@ public void registerCommands() { NamedCommands.registerCommand( "RetractPivot", new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0)); // TODO: add retract angle - // Complex Autos NamedCommands.registerCommand("AutoLogicTest", AutoPaths.testAuto); NamedCommands.registerCommand(