diff --git a/src/main/java/org/curtinfrc/frc2025/BuildConstants.java b/src/main/java/org/curtinfrc/frc2025/BuildConstants.java index 3b96c7b..7397dee 100644 --- a/src/main/java/org/curtinfrc/frc2025/BuildConstants.java +++ b/src/main/java/org/curtinfrc/frc2025/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Reefscape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 17; - public static final String GIT_SHA = "be9b4b89be864857eff3c275a4f3f4c9f4d7727c"; - public static final String GIT_DATE = "2025-01-22 14:40:25 AWST"; + public static final int GIT_REVISION = 42; + public static final String GIT_SHA = "2e09a1eba2cc71b41eddffee8ec070ceb4412e4c"; + public static final String GIT_DATE = "2025-01-24 22:49:02 AWST"; public static final String GIT_BRANCH = "Toby/intake"; - public static final String BUILD_DATE = "2025-01-24 12:05:48 AWST"; - public static final long BUILD_UNIX_TIME = 1737691548380L; + public static final String BUILD_DATE = "2025-01-24 23:28:41 AWST"; + public static final long BUILD_UNIX_TIME = 1737732521172L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java index ce80632..fd24d78 100644 --- a/src/main/java/org/curtinfrc/frc2025/Robot.java +++ b/src/main/java/org/curtinfrc/frc2025/Robot.java @@ -1,6 +1,7 @@ package org.curtinfrc.frc2025; import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.*; +import static org.curtinfrc.subsystems.intake.IntakeConstants.intakeVolts; import choreo.auto.AutoFactory; import com.ctre.phoenix6.SignalLogger; @@ -55,9 +56,9 @@ */ public class Robot extends LoggedRobot { // Subsystems - private final Drive drive; - private final Vision vision; - private final Intake intake; + private Drive drive; + private Vision vision; + private Intake intake; private Elevator elevator; private Superstructure superstructure; @@ -259,6 +260,10 @@ public Robot() { () -> controller.getLeftX(), () -> -controller.getRightX())); + intake.setDefaultCommand(intake.intake(intakeVolts / 4)); + intake.frontSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor)); + intake.backSensor.whileTrue(intake.intake(intakeVolts).until(intake.backSensor.negate())); + // Lock to 0° when A button is held controller .a() diff --git a/src/main/java/org/curtinfrc/subsystems/intake/Intake.java b/src/main/java/org/curtinfrc/subsystems/intake/Intake.java index 7160a43..9276bcf 100644 --- a/src/main/java/org/curtinfrc/subsystems/intake/Intake.java +++ b/src/main/java/org/curtinfrc/subsystems/intake/Intake.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { @@ -10,28 +11,22 @@ public class Intake extends SubsystemBase { public Intake(IntakeIO io) { this.io = io; - // setDefaultCommand(run(() -> io.setIntakeVolts(0))); } + public final Trigger frontSensor = new Trigger(() -> inputs.frontSensor); + public final Trigger backSensor = new Trigger(() -> inputs.backSensor); + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); } - public void setIntakeVolts(double volts) { - io.setIntakeVolts(volts); - } - public Command stop() { - return run(() -> io.setIntakeVolts(0)); - } - - public Command intakeCommand() { - return run(() -> io.setIntakeVolts(6)); + return runOnce(() -> io.setVoltage(0)); } - public Command goToTargetRPM() { - return run(() -> io.achieveRPM()).until(() -> io.intakeAtRPM()); + public Command intake(double volts) { + return run(() -> io.setVoltage(volts)); } } diff --git a/src/main/java/org/curtinfrc/subsystems/intake/IntakeConstants.java b/src/main/java/org/curtinfrc/subsystems/intake/IntakeConstants.java index 95aaa41..c2b21d5 100644 --- a/src/main/java/org/curtinfrc/subsystems/intake/IntakeConstants.java +++ b/src/main/java/org/curtinfrc/subsystems/intake/IntakeConstants.java @@ -1,22 +1,10 @@ package org.curtinfrc.subsystems.intake; public class IntakeConstants { - public static double intakeVolts = 6; + public static double intakeVolts = 8; public static int intakeCurrentLimit = 40; public static double motorReduction = 1.0; - public static double intakePort = 99; - public static double intakeMaxVelocity = 550; - public static double intakeTolerance = 0.01; - // public static double goalRPM = 4000; - public static double goalRPM = 500; - public static double intakeLoopError = 0.1; - - public static double kP = 1; - public static double kI = 0; - public static double kD = 0; - public static double kV = 473; - - public static double intakeID = 99; - - public enum intakeSetPoints {} + public static int intakeMotorId = 99; + public static int intakeFrontSensorPort = 99; + public static int intakeBackSensorPort = 99; } diff --git a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIO.java b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIO.java index e421c06..f4f1e21 100644 --- a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIO.java +++ b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIO.java @@ -7,26 +7,13 @@ public interface IntakeIO { public static class IntakeIOInputs { public double appliedVolts; public double currentAmps; - // public AngularVelocity encoderOutput; - public double encoderOutput; - public double goalRPM = 500; - public boolean intakeAtRPM = false; - // double intakeVolts = IntakeConstants.intakeVolts; + public double positionRotations; + public double angularVelocityRotationsPerMinute; + public boolean frontSensor; + public boolean backSensor; } public default void updateInputs(IntakeIOInputs inputs) {} - public default void achieveRPM() {} - - public default boolean intakeAtRPM() { - return false; - } - - public default void beamBreakState() {} - - public default void setIntakeVolts(double volts) {} - - public default void intake() {} - - public default void stop() {} + public default void setVoltage(double volts) {} } diff --git a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOBeamBreak.java b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOBeamBreak.java deleted file mode 100644 index a858b88..0000000 --- a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOBeamBreak.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.curtinfrc.subsystems.intake; - -import edu.wpi.first.wpilibj.DigitalInput; -import java.util.function.BooleanSupplier; - -public class IntakeIOBeamBreak implements IntakeIO { - private final DigitalInput intakeBeamBreak = new DigitalInput(99); - public BooleanSupplier beamBroken = () -> !intakeBeamBreak.get(); - public BooleanSupplier beamConnected = () -> intakeBeamBreak.get(); - - public IntakeIOBeamBreak() {} - - @Override - public void updateInputs(IntakeIOInputs inputs) {} -} diff --git a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONEO.java b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONEO.java index f03ab6b..36de765 100644 --- a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONEO.java +++ b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONEO.java @@ -1,19 +1,31 @@ package org.curtinfrc.subsystems.intake; -import com.revrobotics.RelativeEncoder; +import static org.curtinfrc.subsystems.intake.IntakeConstants.*; + import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import edu.wpi.first.wpilibj.DigitalInput; public class IntakeIONEO implements IntakeIO { - protected final SparkMax intakeNeo = new SparkMax(99, MotorType.kBrushless); - protected final RelativeEncoder intakeEncoder = intakeNeo.getEncoder(); + private final SparkMax intakeNeo = new SparkMax(intakeMotorId, MotorType.kBrushless); + private final DigitalInput frontSensor = new DigitalInput(intakeFrontSensorPort); + private final DigitalInput backSensor = new DigitalInput(intakeBackSensorPort); + // TODO apply configs public IntakeIONEO() {} @Override public void updateInputs(IntakeIOInputs inputs) { - inputs.encoderOutput = intakeEncoder.getVelocity(); - // inputs.encoderOutput = intakeEncoder.getVelocity(); inputs.appliedVolts = intakeNeo.getBusVoltage() * intakeNeo.getAppliedOutput(); + inputs.currentAmps = intakeNeo.getOutputCurrent(); + inputs.positionRotations = intakeNeo.getEncoder().getPosition(); + inputs.angularVelocityRotationsPerMinute = intakeNeo.getEncoder().getVelocity(); + inputs.frontSensor = frontSensor.get(); + inputs.backSensor = backSensor.get(); + } + + @Override + public void setVoltage(double volts) { + intakeNeo.setVoltage(volts); } } diff --git a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONeoMaxMotion.java b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONeoMaxMotion.java deleted file mode 100644 index 333bd05..0000000 --- a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIONeoMaxMotion.java +++ /dev/null @@ -1,47 +0,0 @@ -package org.curtinfrc.subsystems.intake; - -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; - -public class IntakeIONeoMaxMotion extends IntakeIONEO { - private SparkClosedLoopController intakeController = intakeNeo.getClosedLoopController(); - - public IntakeIONeoMaxMotion() { - var config = new SparkMaxConfig(); - config - .smartCurrentLimit(IntakeConstants.intakeCurrentLimit) - .idleMode(IdleMode.kBrake) - .voltageCompensation(12.0); - - // config - // .encoder - // .positionConversionFactor(2.0 * Math.PI / IntakeConstants.motorReduction) - // .velocityConversionFactor((2.0 * Math.PI) / 60.0 / IntakeConstants.motorReduction) - // .uvwMeasurementPeriod(10) - // .uvwAverageDepth(2); - - config.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI).d(IntakeConstants.kD); - - config.closedLoop.velocityFF(1 / IntakeConstants.kV); - - config - .closedLoop - .maxMotion - .maxVelocity(IntakeConstants.intakeMaxVelocity) - .allowedClosedLoopError(IntakeConstants.intakeTolerance); - } - - @Override - public void achieveRPM() { - intakeController.setReference(IntakeConstants.goalRPM, ControlType.kPosition); - } - - @Override - public boolean intakeAtRPM() { - double angularVelocity = intakeEncoder.getVelocity(); - return IntakeConstants.goalRPM - IntakeConstants.intakeTolerance < angularVelocity - && angularVelocity < IntakeConstants.goalRPM + IntakeConstants.intakeTolerance; - } -} diff --git a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOSim.java b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOSim.java index 9c0dd51..282553c 100644 --- a/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/org/curtinfrc/subsystems/intake/IntakeIOSim.java @@ -1,47 +1,49 @@ package org.curtinfrc.subsystems.intake; -import edu.wpi.first.math.MathUtil; +import static org.curtinfrc.subsystems.intake.IntakeConstants.intakeBackSensorPort; +import static org.curtinfrc.subsystems.intake.IntakeConstants.intakeFrontSensorPort; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class IntakeIOSim implements IntakeIO { - private DCMotor intakeMotor = DCMotor.getNEO(1); private DCMotorSim intakeMotorSim; + private SimDevice frontImpl; + private SimBoolean frontSensor; + private SimDevice backImpl; + private SimBoolean backSensor; + private double volts = 0; public IntakeIOSim() { intakeMotorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(intakeMotor, 0.025, 4.0), intakeMotor); - } - private double appliedVolts = 0.0; + frontImpl = SimDevice.create("IntakeSensorFront", intakeFrontSensorPort); + frontSensor = frontImpl.createBoolean("IsTriggered", Direction.kInput, false); + backImpl = SimDevice.create("IntakeSensorBack", intakeBackSensorPort); + backSensor = backImpl.createBoolean("IsTriggered", Direction.kInput, false); + } @Override public void updateInputs(IntakeIOInputs inputs) { - intakeMotorSim.setInputVoltage(appliedVolts); + intakeMotorSim.setInputVoltage(volts); intakeMotorSim.update(0.02); - // inputs.appliedVolts = appliedVolts; inputs.appliedVolts = intakeMotorSim.getInputVoltage(); - // inputs.encoderOutput = intakeMotorSim.getAngularVelocity(); - inputs.encoderOutput = intakeMotorSim.getAngularVelocityRadPerSec(); + inputs.currentAmps = intakeMotorSim.getCurrentDrawAmps(); + inputs.positionRotations = intakeMotorSim.getAngularPositionRotations(); + inputs.angularVelocityRotationsPerMinute = intakeMotorSim.getAngularVelocityRPM(); + inputs.frontSensor = frontSensor.get(); + inputs.backSensor = backSensor.get(); } @Override - public void setIntakeVolts(double volts) { - appliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + public void setVoltage(double volts) { + this.volts = volts; intakeMotorSim.setInputVoltage(volts); } - - @Override - public void achieveRPM() { - intakeMotorSim.setAngularVelocity(IntakeConstants.goalRPM); - } - - @Override - public boolean intakeAtRPM() { - double angularVelocity = intakeMotorSim.getAngularVelocityRadPerSec(); - return IntakeConstants.goalRPM - IntakeConstants.intakeTolerance < angularVelocity - && angularVelocity < IntakeConstants.goalRPM + IntakeConstants.intakeTolerance; - } }