From 4449adf20bca0c8197c05e0ecdd309db3983b048 Mon Sep 17 00:00:00 2001 From: Ian Shi <69531533+IanShiii@users.noreply.github.com> Date: Wed, 24 Jan 2024 17:25:17 -0500 Subject: [PATCH] add intake subsystem (#9) * 1/22 Co-authored-by: Edgar694 Co-authored-by: Owen-Zeng Co-authored-by: sixuanl Co-authored-by: Weifen Chen Co-authored-by: smiley-axolotl * Wrote method signatures in IntakeImpl to help speed up tomorrow's session * Fix IR logic * Finished IntakeImpl, wrote commands Co-authored-by: Weifen Chen Co-authored-by: smiley-axolotl Co-authored-by: Zixi Feng Co-authored-by: sixuanl Co-authored-by: Edgar694 Co-authored-by: emidudu * Added IntakeAcquireButDontStore Co-authored-by: Weifen Chen Co-authored-by: smiley-axolotl Co-authored-by: Zixi Feng Co-authored-by: sixuanl Co-authored-by: Edgar694 Co-authored-by: emidudu * According to all known laws of aviation , there is no way a bee should fly. Its wings are too small to get its fat little body off the ground. Co-authored-by: Weifen Chen Co-authored-by: smiley-axolotl Co-authored-by: Zixi Feng Co-authored-by: sixuanl Co-authored-by: Edgar694 Co-authored-by: emidudu * The bee, of course, flies anyways because bees dont care what humans think is impossible * Yellow, black. Yellow, black. Yellow, black. Yellow, black. Ooh, black and yellow! * Let's shake it up a little. Barry! Breakfast is ready! Co-authored-by: Weifen Chen Co-authored-by: sixuanl Co-authored-by: vipook * Coming! Hang on a second. Hello? Barry? Adam? Can you believe this is happening? * this was so unneccessary --------- Co-authored-by: Edgar694 Co-authored-by: Owen-Zeng Co-authored-by: sixuanl Co-authored-by: Weifen Chen Co-authored-by: smiley-axolotl Co-authored-by: Zixi Feng Co-authored-by: emidudu Co-authored-by: WennieC Co-authored-by: vipook Co-authored-by: BenG49 <64862590+BenG49@users.noreply.github.com> --- .../com/stuypulse/robot/RobotContainer.java | 2 + .../robot/commands/intake/IntakeAcquire.java | 30 ++++++++ .../commands/intake/IntakeAcquireForever.java | 20 +++++ .../commands/intake/IntakeDeacquire.java | 19 +++++ .../robot/commands/intake/IntakeStop.java | 20 +++++ .../com/stuypulse/robot/constants/Motors.java | 7 +- .../com/stuypulse/robot/constants/Ports.java | 5 ++ .../stuypulse/robot/constants/Settings.java | 11 ++- .../robot/subsystems/intake/Intake.java | 23 ++++++ .../robot/subsystems/intake/IntakeImpl.java | 76 +++++++++++++++++++ 10 files changed, 210 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1603c075..cd277c6d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.conveyor.Conveyor; import com.stuypulse.stuylib.input.Gamepad; @@ -23,6 +24,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Intake intake = Intake.getInstance(); public final Shooter shooter = Shooter.getInstance(); public final Conveyor conveyor = Conveyor.getInstance(); diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java new file mode 100644 index 00000000..d57f1738 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.Command; + +public class IntakeAcquire extends Command{ + + private Intake intake; + + public IntakeAcquire() { + intake = Intake.getInstance(); + addRequirements(intake); + } + + @Override + public void initialize() { + intake.acquire(); + } + + @Override + public void end(boolean interrupted) { + intake.stop(); + } + + @Override + public boolean isFinished() { + return intake.hasNote(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java new file mode 100644 index 00000000..62d91027 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class IntakeAcquireForever extends InstantCommand { + + private Intake intake; + + public IntakeAcquireForever() { + intake = Intake.getInstance(); + addRequirements(intake); + } + + @Override + public void initialize() { + intake.acquire(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java new file mode 100644 index 00000000..661da577 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class IntakeDeacquire extends InstantCommand { + private Intake intake; + + public IntakeDeacquire() { + intake = Intake.getInstance(); + addRequirements(intake); + } + + @Override + public void initialize() { + intake.deacquire(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java new file mode 100644 index 00000000..1c479d3a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +import com.stuypulse.robot.subsystems.intake.Intake; + +public class IntakeStop extends InstantCommand { + + private Intake intake; + + public IntakeStop() { + intake = Intake.getInstance(); + addRequirements(intake); + } + + @Override + public void initialize() { + intake.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 1a458bf1..0edf0fcb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -24,13 +24,16 @@ public interface Motors { /** Classes to store all of the values a motor needs */ + public interface Intake { + CANSparkMaxConfig MOTOR_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 80, 0.1); + } + public interface Shooter { CANSparkMaxConfig LEFT_SHOOTER = new CANSparkMaxConfig(false,IdleMode.kCoast); CANSparkMaxConfig RIGHT_SHOOTER = new CANSparkMaxConfig(false,IdleMode.kCoast); } - public interface Conveyor{ - + public interface Conveyor { CANSparkMaxConfig GANDALF_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake); CANSparkMaxConfig SHOOTER_FEEDER_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast); } diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 03068554..c81051d4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,6 +12,11 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } + + public interface Intake { + int MOTOR = 40; + int SENSOR = 1; + } public interface Shooter { int LEFT_MOTOR = 20; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2613d940..826d84ea 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -35,6 +35,16 @@ public interface PID { } } + public interface Intake { + public interface Detection { + SmartNumber TRIGGER_TIME = new SmartNumber("Intake/Trigger Debounce Time", 0.05); + SmartNumber STALL_TIME = new SmartNumber("Intake/Stall Debounce Time", .05); + SmartNumber STALL_CURRENT = new SmartNumber("Intake/Stall Current", 40); + } + SmartNumber ACQUIRE_SPEED = new SmartNumber("Intake/Acquire", 1); + SmartNumber DEACQUIRE_SPEED = new SmartNumber("Intake/Deacquire", -1); + } + public interface Conveyor { SmartNumber GANDALF_SHOOTER_SPEED = new SmartNumber("Conveyor/Gandalf Shooter Speed", 1); SmartNumber GANDALF_AMP_SPEED = new SmartNumber("Conveyor/Gandalf Amp Speed", -1); @@ -42,5 +52,4 @@ public interface Conveyor { SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.2); } - } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java new file mode 100644 index 00000000..0f7e9b06 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Intake extends SubsystemBase { + + private static final Intake instance; + + static { + instance = new IntakeImpl(); + } + + public static Intake getInstance() { + return instance; + } + + public abstract void acquire(); + public abstract void deacquire(); + public abstract void stop(); + + public abstract boolean hasNote(); + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java new file mode 100644 index 00000000..a4152253 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -0,0 +1,76 @@ +package com.stuypulse.robot.subsystems.intake; + +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class IntakeImpl extends Intake { + + private final CANSparkMax motor; + private final DigitalInput sensor; + + private final BStream triggered; + private final BStream stalling; + + public IntakeImpl() { + motor = new CANSparkMax(Ports.Intake.MOTOR, MotorType.kBrushless); + sensor = new DigitalInput(Ports.Intake.SENSOR); + + Motors.Intake.MOTOR_CONFIG.configure(motor); + + triggered = BStream.create(sensor).not().filtered(new BDebounce.Both(Settings.Intake.Detection.TRIGGER_TIME)); + stalling = BStream.create(this::isMomentarilyStalling).filtered(new BDebounce.Both(Settings.Intake.Detection.STALL_TIME)); + } + + @Override + public void acquire() { + motor.set(Settings.Intake.ACQUIRE_SPEED.getAsDouble()); + } + + @Override + public void deacquire() { + motor.set(Settings.Intake.DEACQUIRE_SPEED.getAsDouble()); + } + + @Override + public void stop() { + motor.stopMotor(); + } + + // Detection + + private boolean isMomentarilyStalling() { + return motor.getOutputCurrent() > Settings.Intake.Detection.STALL_CURRENT.getAsDouble(); + } + + private boolean isStalling() { + return stalling.get(); + } + + private boolean isTriggered() { + return triggered.get(); + } + + // Not using stall detection, but keeping it as an option + @Override + public boolean hasNote() { + return isTriggered(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Intake/Speed", motor.get()); + SmartDashboard.putNumber("Intake/Current", motor.getOutputCurrent()); + + SmartDashboard.putBoolean("Intake/Is Stalling", isStalling()); + SmartDashboard.putBoolean("Intake/IR is triggered", isTriggered()); + } +}