From 61e7def26fae8241818a7ea7d591e8db3ce37695 Mon Sep 17 00:00:00 2001 From: Papplefarm20 <146298520+Papplefarm20@users.noreply.github.com> Date: Fri, 26 Jan 2024 18:57:15 -0800 Subject: [PATCH] feat: Intake & Indexer IO and subsystem --- .../org/team1540/robot2024/Constants.java | 7 ++ .../commands/indexer/IntakeCommand.java | 30 ++++++++ .../robot2024/subsystems/indexer/Indexer.java | 26 ++++++- .../subsystems/indexer/IndexerIO.java | 14 ++-- .../subsystems/indexer/IndexerIOReal.java | 34 --------- .../subsystems/indexer/IndexerIOSparkMax.java | 74 +++++++++++++++++++ 6 files changed, 142 insertions(+), 43 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java delete mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 3703ba85..a9194659 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -57,6 +57,13 @@ public static class Drivetrain { } public static class Indexer { public static final int INTAKE_ID = 0; + public static final int FEEDER_ID = 0; + public static final double FEEDER_KP = 0.0; + public static final double FEEDER_KI = 0.0; + public static final double FEEDER_KD = 0.0; + public static final double FEEDER_KS = 0.0; + public static final double FEEDER_KV = 0.0; + public static final double GEAR_RATIO = 0.0; } } diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java new file mode 100644 index 00000000..68af9d5e --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java @@ -0,0 +1,30 @@ +package org.team1540.robot2024.commands.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.robot2024.subsystems.indexer.Indexer; + +public class IntakeCommand extends Command { + + private final Indexer indexer; + + public IntakeCommand(Indexer indexer) { + this.indexer = indexer; + addRequirements(indexer); + } + + @Override + public void initialize() { + indexer.setIntakePercent(0.4); + } + + + @Override + public boolean isFinished() { + return indexer.noteInIndexer(); + } + + @Override + public void end(boolean interrupted) { + indexer.setIntakePercent(0); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index f3facc50..3688812b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -1,5 +1,8 @@ package org.team1540.robot2024.subsystems.indexer; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -17,14 +20,29 @@ public void periodic() { Logger.processInputs("Indexer", indexerInputs); } - public void setIntakeSpeed(double speed) { - indexerIO.setIntakeSpeed(speed); + public void setIntakePercent(double percent) { + indexerIO.setIntakeVoltage(percent * 12.0); } - public void setIntakeVoltage(double volts) { - indexerIO.setIntakeVoltage(volts); + public boolean noteInIndexer() { + return indexerInputs.noteInIntake; } + public Command feedToAmp() { + return Commands.sequence( + Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this), + Commands.waitSeconds(5), + Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) + ); + } + + public Command feedToShooter() { + return Commands.sequence( + Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this), + Commands.waitSeconds(1), + Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this) + ); + } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index 23266270..a65aeab8 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -6,9 +6,13 @@ public interface IndexerIO { @AutoLog class IndexerIOInputs { - public double intakeVoltage = 0.0; - public double intakeCurrent = 0.0; - public double intakeVelocityRPM = 0.0; + public double intakeVoltage; + public double intakeCurrent; + public double intakeVelocityRPM; + public double feederVoltage; + public double feederCurrentAmps; + public double feederVelocityRPM; + public boolean noteInIntake; } /** @@ -18,8 +22,8 @@ default void updateInputs(IndexerIOInputs inputs) {} default void setIntakeVoltage(double volts) {} - default void setIntakeSpeed(double speed) {} - + default void setFeederVoltage(double volts) {} + default void setFeederVelocity(double velocity) {} } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java deleted file mode 100644 index 849aa338..00000000 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.team1540.robot2024.subsystems.indexer; - -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; - -import static org.team1540.robot2024.Constants.Indexer.*; - -public class IndexerIOReal implements IndexerIO { - CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); - - - public IndexerIOReal() { - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); - intakeMotor.enableVoltageCompensation(12.0); - intakeMotor.setSmartCurrentLimit(30); - } - - @Override - public void updateInputs(IndexerIOInputs inputs) { - inputs.intakeCurrent = intakeMotor.getOutputCurrent(); - inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); - } - - @Override - public void setIntakeVoltage(double volts) { - intakeMotor.setVoltage(volts); - } - - @Override - public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); - } -} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java new file mode 100644 index 00000000..7e30bcd0 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -0,0 +1,74 @@ +package org.team1540.robot2024.subsystems.indexer; + +import com.revrobotics.*; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; + +import static org.team1540.robot2024.Constants.Indexer.*; + + +public class IndexerIOSparkMax implements IndexerIO { + private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless); + private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless); + DigitalInput indexerBeamBreak = new DigitalInput(0); + private final SparkPIDController feederPID; + private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); + + + public IndexerIOSparkMax() { + intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + intakeMotor.enableVoltageCompensation(12.0); + intakeMotor.setSmartCurrentLimit(30); + + feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + feederMotor.enableVoltageCompensation(12.0); + feederMotor.setSmartCurrentLimit(30); + + feederPID = feederMotor.getPIDController(); + feederPID.setP(FEEDER_KP, 0); + feederPID.setI(FEEDER_KI, 0); + feederPID.setD(FEEDER_KD, 0); + + + + } + // 2 jobs, + // 1. be able to route to tramp + // 2. feeding the shooter (by default, be still, spin up fast 4 for shooter. + // make cmd factory spin + // slower in other direction for tramp + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.intakeCurrent = intakeMotor.getOutputCurrent(); + inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); + inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity(); + inputs.feederVoltage = feederMotor.getOutputCurrent(); + inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); + inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); + inputs.noteInIntake = indexerBeamBreak.get(); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeMotor.setVoltage(volts); + } + + @Override + public void setFeederVoltage(double volts) { + feederMotor.setVoltage(volts); + } + + @Override + public void setFeederVelocity(double velocity) { + feederPID.setReference( + Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO, + CANSparkBase.ControlType.kVelocity, + 0, + feederFF.calculate(velocity), + SparkPIDController.ArbFFUnits.kVoltage + ); + } +} +// for elevator sim, app. voltage \ No newline at end of file