diff --git a/src/main/java/org/team1540/robot2023/Constants.java b/src/main/java/org/team1540/robot2023/Constants.java index 0a2b95d..c52d748 100644 --- a/src/main/java/org/team1540/robot2023/Constants.java +++ b/src/main/java/org/team1540/robot2023/Constants.java @@ -265,5 +265,7 @@ public static final class RollerIntakeConstants { public static final int ROLLER_INTAKE_ID = 16; public static final int INTAKE_IDLE_CURRENT = 10; // TODO: 8/29/2023 find the intake idle current limit public static final int INTAKE_AGGRO_CURRENT = 30; // TODO: 8/29/2023 find the intake aggravating current limit + public static final double VELOCITY_THRESH = 200; // TODO: 9/8/2023 figure out velocity threshold for hasGamePiece + } } \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2023/commands/wristintake/RollerIntake.java b/src/main/java/org/team1540/robot2023/commands/wristintake/RollerIntake.java new file mode 100644 index 0000000..8fb6002 --- /dev/null +++ b/src/main/java/org/team1540/robot2023/commands/wristintake/RollerIntake.java @@ -0,0 +1,40 @@ +package org.team1540.robot2023.commands.wristintake; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.team1540.robot2023.utils.AverageFilter; + +import static org.team1540.robot2023.Constants.RollerIntakeConstants.*; + +public class RollerIntake extends SubsystemBase { + private final TalonFX motor = new TalonFX(ROLLER_INTAKE_ID); + private final AverageFilter averageFilter = new AverageFilter(5); + + public RollerIntake() { + motor.configStatorCurrentLimit( + new StatorCurrentLimitConfiguration(true, INTAKE_IDLE_CURRENT, INTAKE_IDLE_CURRENT, 0) + ); + motor.setNeutralMode(NeutralMode.Brake); + motor.setInverted(false); // TODO: 8/29/2023 figure out inversion + } + + public void setSpeed(double speed) { + motor.set(ControlMode.PercentOutput, speed); + } + + public void stop() { + setSpeed(0); + } + + public boolean hasGamePiece() { + return averageFilter.getAverage() < VELOCITY_THRESH; + } + + @Override + public void periodic() { + averageFilter.add(motor.getSelectedSensorVelocity()); + } +}