diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 64bc1e70..e0a76611 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -18,7 +18,7 @@ public final class Constants { // Whether to pull PID constants from SmartDashboard public static final boolean tuningMode = true; // TODO: DO NOT SET TO TRUE FOR COMP private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY - + public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; public enum Mode { @@ -63,6 +63,23 @@ public static class Drivetrain { public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } + public static class Indexer { + // TODO: fix these constants + public static final int INTAKE_ID = 11; + public static final int FEEDER_ID = 12; + public static final double FEEDER_KP = 0.5; + public static final double FEEDER_KI = 0.1; + public static final double FEEDER_KD = 0.001; + public static final double FEEDER_KS = 0.0; + public static final double FEEDER_KV = 0.0; + public static final double FEEDER_GEAR_RATIO = 1.0; + public static final double INTAKE_GEAR_RATIO = 1.0; + public static final double INTAKE_MOI = 0.025; + public static final double FEEDER_MOI = 0.025; + public static final int BEAM_BREAK_ID = 0; + + + } public static class Vision { diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 951c9c70..0237920f 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -15,6 +15,7 @@ import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; +import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.elevator.ElevatorIO; @@ -30,6 +31,10 @@ import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight; import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim; import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; +import org.team1540.robot2024.subsystems.indexer.Indexer; +import org.team1540.robot2024.subsystems.indexer.IndexerIO; +import org.team1540.robot2024.subsystems.indexer.IndexerIOSim; +import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.team1540.robot2024.util.vision.VisionPoseAcceptor; @@ -48,6 +53,7 @@ public class RobotContainer { public final Tramp tramp; public final Shooter shooter; public final Elevator elevator; + public final Indexer indexer; public final AprilTagVision aprilTagVision; // Controller @@ -80,6 +86,10 @@ public RobotContainer() { tramp = new Tramp(new TrampIOSparkMax()); shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); elevator = new Elevator(new ElevatorIOTalonFX()); + indexer = + new Indexer( + new IndexerIOSparkMax() + ); aprilTagVision = new AprilTagVision( new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE), new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE), @@ -106,6 +116,10 @@ public RobotContainer() { ignored -> {}, () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE + indexer = + new Indexer( + new IndexerIOSim() + ); break; default: // Replayed robot, disable IO implementations @@ -125,7 +139,14 @@ public RobotContainer() { (ignored) -> {}, () -> 0.0, new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); + + indexer = + new Indexer( + new IndexerIO() {} + ); + tramp = new Tramp(new TrampIO() {}); + break; } @@ -168,6 +189,8 @@ private void configureButtonBindings() { copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get())) .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter)); + + driver.a().onTrue(new IntakeCommand(indexer)); } /** 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..72df4da5 --- /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.isNotePresent(); + } + + @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 new file mode 100644 index 00000000..37afbc62 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -0,0 +1,51 @@ +package org.team1540.robot2024.subsystems.indexer; + +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; +import org.team1540.robot2024.util.LoggedTunableNumber; +import static org.team1540.robot2024.Constants.Indexer.*; +import static org.team1540.robot2024.Constants.tuningMode; + + +public class Indexer extends SubsystemBase { + private final IndexerIO indexerIO; + private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged(); + private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD); + + + public Indexer(IndexerIO indexerIO) { + this.indexerIO = indexerIO; + } + + public void periodic() { + indexerIO.updateInputs(indexerInputs); + Logger.processInputs("Indexer", indexerInputs); + if (tuningMode) { + if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { + indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get()); + } + } + } + + public void setIntakePercent(double percent) { + indexerIO.setIntakeVoltage(percent * 12.0); + } + + public boolean isNotePresent() { + return indexerInputs.noteInIntake; + } + + public Command feedToAmp() { + return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this); + } + + public Command feedToShooter() { + return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), 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 new file mode 100644 index 00000000..f08a1858 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -0,0 +1,33 @@ +package org.team1540.robot2024.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + + @AutoLog + class IndexerIOInputs { + public double intakeVoltage; + public double intakeCurrentAmps; + public double intakeVelocityRPM; + public double feederVoltage; + public double feederCurrentAmps; + public double feederVelocityRPM; + public boolean noteInIntake = false; + public double setpointRPM; + public double feederVelocityError; + } + + /** + * Updates the set of loggable inputs. + */ + default void updateInputs(IndexerIOInputs inputs) {} + + default void setIntakeVoltage(double volts) {} + + default void setFeederVoltage(double volts) {} + + default void setFeederVelocity(double velocity) {} + + default void configureFeederPID(double p, double i, double d) {} + +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java new file mode 100644 index 00000000..ea11766f --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -0,0 +1,66 @@ +package org.team1540.robot2024.subsystems.indexer; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Indexer.*; + +public class IndexerIOSim implements IndexerIO { + + + private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI); + private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI); +// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); + private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD); + private boolean isClosedLoop = true; + private double intakeVoltage = 0.0; + private double feederVoltage = 0.0; + private double feederSetpointRPS = 0.0; + + @Override + public void updateInputs(IndexerIOInputs inputs) { + if (isClosedLoop) { + feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederSetpointRPS), -12.0, 12.0); + } + intakeSim.setInputVoltage(intakeVoltage); + feederSim.setInputVoltage(feederVoltage); + intakeSim.update(Constants.LOOP_PERIOD_SECS); + feederSim.update(Constants.LOOP_PERIOD_SECS); + inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps(); + inputs.intakeVoltage = intakeVoltage; + inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM(); + inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); + inputs.feederVoltage = feederVoltage; + inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); +// inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); + inputs.setpointRPM = feederSimPID.getSetpoint() * 60; + inputs.feederVelocityError = feederSimPID.getPositionError(); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeVoltage = MathUtil.clamp(volts, -12.0, 12.0); + } + + @Override + public void configureFeederPID(double p, double i, double d) { + feederSimPID.setPID(p, i, d); + } + + public void setFeederVoltage(double volts) { + isClosedLoop = false; + feederVoltage = MathUtil.clamp(volts, -12.0, 12.0); + } + + + public void setFeederVelocity(double velocity) { + isClosedLoop = true; + feederSimPID.reset(); + feederSetpointRPS = velocity / 60; + } + +} 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..18c9686b --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -0,0 +1,75 @@ +package org.team1540.robot2024.subsystems.indexer; + +import com.revrobotics.*; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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); + private final DigitalInput indexerBeamBreak = new DigitalInput(BEAM_BREAK_ID); + private final SparkPIDController feederPID; + private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV); + private double setpointRPM; + + + 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); + } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.intakeCurrentAmps = intakeMotor.getOutputCurrent(); + inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput(); + inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity(); + inputs.feederCurrentAmps = feederMotor.getOutputCurrent(); + inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput(); + inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); + inputs.noteInIntake = indexerBeamBreak.get(); + inputs.setpointRPM = setpointRPM; + inputs.feederVelocityError = setpointRPM - feederMotor.getEncoder().getVelocity(); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeMotor.setVoltage(volts); + } + + @Override + public void setFeederVoltage(double volts) { + feederMotor.setVoltage(volts); + } + + @Override + public void setFeederVelocity(double velocity) { + setpointRPM = velocity; + feederPID.setReference( + velocity / FEEDER_GEAR_RATIO, // Should this be multiplied? + CANSparkBase.ControlType.kVelocity, + 0, + feederFF.calculate(velocity), + SparkPIDController.ArbFFUnits.kVoltage + ); + } + + @Override + public void configureFeederPID(double p, double i, double d) { + feederPID.setP(p); + feederPID.setI(i); + feederPID.setD(d); + } +}