Source code

001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.subsystems.intake;
007
008import java.util.Optional;
009import java.util.function.BooleanSupplier;
010import com.ctre.phoenix6.controls.ControlRequest;
011import com.ctre.phoenix6.controls.DutyCycleOut;
012import com.ctre.phoenix6.controls.Follower;
013import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
014import com.ctre.phoenix6.controls.TorqueCurrentFOC;
015import com.ctre.phoenix6.controls.VoltageOut;
016import com.ctre.phoenix6.hardware.TalonFX;
017import com.ctre.phoenix6.signals.MotorAlignmentValue;
018import com.stuypulse.robot.constants.Motors;
019import com.stuypulse.robot.constants.Ports;
020import com.stuypulse.robot.constants.Settings;
021import com.stuypulse.robot.util.LoggedSignals;
022import com.stuypulse.robot.util.SysId;
023
024import static edu.wpi.first.units.Units.*;
025import edu.wpi.first.units.measure.*;
026
027import dev.doglog.DogLog;
028
029import edu.wpi.first.math.filter.Debouncer;
030import edu.wpi.first.math.filter.Debouncer.DebounceType;
031
032import edu.wpi.first.wpilibj.DigitalInput;
033import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
034
035public class IntakeImpl extends Intake {
036
037    private final DigitalInput pivotLimitSwitch;
038    
039    private final TalonFX pivotMotor;
040
041    private final TalonFX rollerMotorLeft;
042
043    private final TalonFX rollerMotorRight;
044
045    private final PositionTorqueCurrentFOC positionController;
046
047    private final VoltageOut homingController;
048
049    private final TorqueCurrentFOC pushdownController;
050
051    private final DutyCycleOut rollerController;
052
053    private final Follower followerController;
054
055    private final LoggedSignals pivotSignals;
056
057    private final LoggedSignals rollerSignals;
058
059    private final BooleanSupplier pivotStalling;
060    private final BooleanSupplier leftRollerStalling;
061    private final BooleanSupplier rightRollerStalling;
062
063    private Optional<Voltage> pivotVoltageOverride;
064
065    private final Debouncer leftRollerDebouncer;
066    private final Debouncer rightRollerDebouncer;
067
068    public IntakeImpl() {
069        pivotLimitSwitch = new DigitalInput(Ports.Intake.PIVOT_LIMIT_SWITCH);
070        pivotMotor = new TalonFX(Ports.Intake.INTAKE_PIVOT_MOTOR, Settings.CANBUS);
071        Motors.Intake.PIVOT_CONFIG.configure(pivotMotor);
072
073        // zero it at the up pos
074        pivotMotor.setPosition(Settings.Intake.Pivot.INITIAL_ANGLE);
075        pivotSignals = new LoggedSignals(LoggedSignals.SignalLocation.CANIVORE, "Intake/Pivot/",
076                pivotMotor.getSupplyCurrent(),
077                pivotMotor.getStatorCurrent(),
078                pivotMotor.getVelocity());
079
080            // leader
081        rollerMotorLeft = new TalonFX(Ports.Intake.INTAKE_ROLLER_MOTOR_LEFT, Settings.CANBUS);
082        rollerMotorRight = new TalonFX(Ports.Intake.INTAKE_ROLLER_MOTOR_RIGHT, Settings.CANBUS);
083        Motors.Intake.LEFT_ROLLER_CONFIG.configure(rollerMotorLeft);
084        Motors.Intake.RIGHT_ROLLER_CONFIG.configure(rollerMotorRight);
085        // until rollers are fixed
086        rollerSignals = new LoggedSignals(LoggedSignals.SignalLocation.CANIVORE, "Intake/Roller/").withMotor(
087                "Intake Roller Left",
088                rollerMotorLeft.getSupplyCurrent(),
089                rollerMotorLeft.getStatorCurrent(),
090                rollerMotorLeft.getVelocity())
091                .withMotor(
092                        "Intake Roller Right",
093                        rollerMotorRight.getSupplyCurrent(),
094                        rollerMotorRight.getStatorCurrent(),
095                        rollerMotorRight.getVelocity());
096        positionController = new PositionTorqueCurrentFOC(getState().getTargetAngle());
097        homingController = new VoltageOut(Settings.Intake.Pivot.HOMING_DOWN_VOLTAGE).withEnableFOC(true);
098        pushdownController = new TorqueCurrentFOC(Settings.Intake.Pivot.PUSHDOWN_CURRENT.getAsDouble());
099        rollerController = new DutyCycleOut(getState().getTargetDutyCycle()).withEnableFOC(true);
100        followerController = new Follower(Ports.Intake.INTAKE_ROLLER_MOTOR_LEFT, MotorAlignmentValue.Opposed);
101        rollerMotorRight.setControl(followerController);
102        pivotStalling = () -> pivotMotor.getStatorCurrent().getValue().gt(Settings.Intake.Pivot.STALL_CURRENT);
103        leftRollerStalling = () -> rollerMotorLeft.getStatorCurrent().getValue().gt(Settings.Intake.Roller.STALL_CURRENT);
104        rightRollerStalling = () -> rollerMotorRight.getStatorCurrent().getValue().gt(Settings.Intake.Roller.STALL_CURRENT);
105        // until rollers are fixed
106        leftRollerDebouncer = new Debouncer(Settings.Intake.Roller.STALL_DEBOUNCE_SEC.in(Seconds), DebounceType.kBoth);
107        rightRollerDebouncer = new Debouncer(Settings.Intake.Roller.STALL_DEBOUNCE_SEC.in(Seconds), DebounceType.kBoth);
108        pivotVoltageOverride = Optional.empty();
109    }
110
111    // For sysid
112    @Override
113    public void setPivotVoltageOverride(Voltage voltage) {
114        this.pivotVoltageOverride = Optional.of(voltage);
115    }
116
117    /** ****************** */
118    /** Pivot Commands ** */
119    /** ****************** */
120    
121    @Override
122    public boolean limitSwitchHit() {
123        return !pivotLimitSwitch.get();
124    }
125    
126    @Override
127    public Angle getRelativePosition() {
128        return pivotMotor.getPosition().getValue();
129    }
130
131    @Override
132    public void seedPivotAngle(Angle angle) {
133        pivotMotor.setPosition(angle);
134    }
135
136    private boolean pivotStalling() {
137        return pivotStalling.getAsBoolean();
138    }
139
140    /** ******************* */
141    /** Roller Commands ** */
142    /** ******************* */
143    // until rollers are fixed
144    @Override
145    public AngularVelocity getRollerVelocity() {
146        return rollerMotorRight.getVelocity().getValue();
147    }
148
149    private boolean getLeftRollerStalling() {
150        return leftRollerDebouncer.calculate(leftRollerStalling.getAsBoolean());
151    }
152
153    private boolean getRightRollerStalling() {
154        return rightRollerDebouncer.calculate(rightRollerStalling.getAsBoolean());
155    }
156
157    @Override
158    protected void stopRollerMotors() {
159        rollerMotorLeft.stopMotor();
160        rollerMotorRight.stopMotor();
161        // re-add the follow control after stopMotor removes it
162        rollerMotorRight.setControl(followerController);
163    }
164
165    @Override
166    protected void stopPivotMotor() {
167        pivotMotor.stopMotor();
168    }
169
170    private ControlRequest getPivotControl(IntakeState currentState) {
171        return switch (currentState) {
172            case INTAKE, OUTTAKE, DOWN -> pushdownController; //(pivotAboveThreshold) // Hardcoding pushdown disabled temporarily - AZ
173                    // ? pushdownController.withOutput(Settings.Intake.Pivot.PUSHDOWN_CURRENT.getAsDouble())
174                    // : positionController.withPosition(currentState.getTargetAngle()).withSlot(0);
175            case HOMING_DOWN -> homingController;
176            case AGITATE, AGITATE_DOWN -> positionController.withPosition(currentState.getTargetAngle()).withSlot(1);
177            default -> positionController.withPosition(currentState.getTargetAngle()).withSlot(0);
178        };
179    }
180
181    @Override
182    public void periodic() {
183        if (!Settings.EnabledSubsystems.INTAKE.get()) {
184            stopAllMotors();
185
186            return;
187        }
188
189        if (pivotVoltageOverride.isPresent()) {
190            pivotMotor.setVoltage(pivotVoltageOverride.get().in(Volts));
191            return;
192        }
193
194        // Input
195        final boolean pivotStalling = pivotStalling();
196        final boolean leftRollerStalling = getLeftRollerStalling();
197        final boolean rightRollerStalling = getRightRollerStalling();
198        final IntakeState currentState = getState();
199
200        // State
201        if (limitSwitchHit()) {
202            seedPivotAngle(Settings.Intake.Pivot.DEPLOY_ANGLE);
203        }
204
205        if (currentState == IntakeState.HOMING_DOWN && (pivotStalling || limitSwitchHit())) {
206            seedPivotAngle(Settings.Intake.Pivot.DEPLOY_ANGLE);
207            setState(IntakeState.INTAKE);
208        }
209        if ((currentState == IntakeState.DOWN) && (pivotStalling || limitSwitchHit())) {
210            seedPivotAngle(Settings.Intake.Pivot.DEPLOY_ANGLE);
211        }
212
213        // Output
214        final ControlRequest pivotControl = getPivotControl(currentState);
215        final DutyCycleOut rollerControl = rollerController.withOutput(currentState.getTargetDutyCycle());
216
217        // Apply
218        pivotMotor.setControl(pivotControl);
219
220        rollerMotorLeft.setControl(rollerControl);
221
222        // Logging
223        this.pivotSignals.logAll();
224        this.rollerSignals.logAll();
225        DogLog.forceNt.log("Intake/Rollers/Stalling", leftRollerStalling || rightRollerStalling);
226        DogLog.forceNt.log(
227                "Intake/Pivot/Pushing Down", pivotMotor.getAppliedControl() == pushdownController);
228        super.periodic();
229    }
230
231    @Override
232    public SysIdRoutine getIntakeSysIdRoutine() {
233        return SysId.getRoutine(
234                Settings.Intake.Pivot.RAMP_RATE,
235                Settings.Intake.Pivot.STEP_VOLTAGE,
236                "Intake",
237                this::setPivotVoltageOverride,
238                () -> pivotMotor.getPosition().getValue(),
239                () -> pivotMotor.getVelocity().getValue(),
240                () -> pivotMotor.getMotorVoltage().getValue(),
241                getInstance());
242    }
243}