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 com.stuypulse.robot.Robot;
009import com.stuypulse.robot.commands.intake.IntakeSeedPivotDeployed;
010import com.stuypulse.robot.commands.intake.IntakeSeedPivotNinety;
011import com.stuypulse.robot.commands.intake.IntakeSeedPivotStowed;
012import com.stuypulse.robot.constants.Settings;
013
014import dev.doglog.DogLog;
015import static edu.wpi.first.units.Units.Degrees;
016import static edu.wpi.first.units.Units.RPM;
017import static edu.wpi.first.units.Units.Rotations;
018import edu.wpi.first.units.measure.Angle;
019import edu.wpi.first.units.measure.AngularVelocity;
020import edu.wpi.first.units.measure.Voltage;
021import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
022import edu.wpi.first.wpilibj2.command.SubsystemBase;
023import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
024
025public abstract class Intake extends SubsystemBase {
026
027    private static final Intake instance;
028
029    private IntakeState state;
030
031    static {
032        if (Robot.isReal()) {
033            instance = new IntakeImpl();
034        } else {
035            instance = new IntakeSim();
036        }
037        // Elastic Commands
038        SmartDashboard.putData("Intake/Seed Pivot Angle Stowed", new IntakeSeedPivotStowed());
039        SmartDashboard.putData("Intake/Set Pivot Angle Deployed", new IntakeSeedPivotDeployed());
040        SmartDashboard.putData("Intake/Seed Pivot Angle 90", new IntakeSeedPivotNinety());
041    }
042
043    public static Intake getInstance() {
044        return instance;
045    }
046
047    public abstract boolean limitSwitchHit();
048
049    /** Enum representing the different possible states of the intake. */
050    public enum IntakeState {
051
052        AGITATE_DOWN(Settings.Intake.Pivot.AGITATE_DOWN_ANGLE, Settings.Intake.Roller.INTAKE_DUTY_CYCLE),
053        /** The intake is stowed and rollers are off. */
054        IDLE(Settings.Intake.Pivot.STOW_ANGLE, 0),
055        /** The intake is deployed but rollers are off. */
056        DOWN(Settings.Intake.Pivot.DEPLOY_ANGLE, 0),
057        /** The intake is deployed and rollers are running to take in gamepieces. */
058        INTAKE(Settings.Intake.Pivot.DEPLOY_ANGLE, Settings.Intake.Roller.INTAKE_DUTY_CYCLE),
059        /**
060         * The intake is deployed and rollers are running in reverse to expel
061         * gamepieces.
062         */
063        OUTTAKE(Settings.Intake.Pivot.DEPLOY_ANGLE, Settings.Intake.Roller.OUTTAKE_DUTY_CYCLE),
064        /**
065         * The intake is brought up repeatedly to an angle between stowed and deployed
066         * to dislodge
067         * gamepieces. Rollers do not run.
068         */
069        AGITATE(Settings.Intake.Pivot.AGITATE_UP_ANGLE, Settings.Intake.Roller.INTAKE_DUTY_CYCLE),
070        
071        /**
072         * The intake is brought up once to an angle between stowed and deployed to
073         * dislodge gamepieces.
074         * Rollers do not run.
075         */
076        DIGEST(Settings.Intake.Pivot.DIGEST_ANGLE, 0),
077        /** The intake is pushed against the bumpers to re-zero the pivot. */
078        HOMING_DOWN(Settings.Intake.Pivot.DEPLOY_ANGLE, 0);
079
080        /** The target angle of the intake pivot. */
081        private Angle targetAngle;
082
083        /** The target percentage of voltage of the intake rollers. */
084        private double targetDutyCycle;
085
086        /**
087         * Constructs an IntakeState with its target values.
088         *
089         * @param targetAngle     In any unit, the target position of the intake pivot
090         * @param targetDutyCycle In any unit, the target percentage of voltage of the
091         *                        intake rollers
092         */
093        private IntakeState(Angle targetAngle, double targetDutyCycle) {
094            this.targetAngle = targetAngle;
095            this.targetDutyCycle = targetDutyCycle;
096        }
097
098        /**
099         * Gets the target position of the pivot.
100         *
101         * @return the target angle
102         */
103        public Angle getTargetAngle() {
104            return targetAngle;
105        }
106
107        /**
108         * Gets the target position of the pivot.
109         *
110         * @return the target percentage of voltage of the intake rollers
111         */
112        public double getTargetDutyCycle() {
113            return targetDutyCycle;
114        }
115    }
116
117    protected Intake() {
118        this.state = IntakeState.IDLE;
119    }
120
121    public void setState(IntakeState state) {
122        this.state = state;
123    }
124
125    public IntakeState getState() {
126        return state;
127    }
128
129    // Pivot Commands
130    public abstract Angle getRelativePosition();
131
132    public abstract void seedPivotAngle(Angle angle);
133
134    public boolean atTargetAngle() {
135        return getRelativePosition().minus(getState().getTargetAngle())
136                .abs(Rotations) < Settings.Intake.Pivot.ANGLE_TOLERANCE.in(Rotations);
137    }
138
139    public boolean isPivotAboveThreshold() {
140        return getRelativePosition().gt(Settings.Intake.Pivot.PUSHDOWN_THRESHOLD);
141    }
142
143    // Roller Commands
144    public abstract AngularVelocity getRollerVelocity();
145
146    // Stop Commands
147    protected abstract void stopRollerMotors();
148    protected abstract void stopPivotMotor();
149    protected void stopAllMotors() {
150        stopRollerMotors();
151        stopPivotMotor();
152    };
153
154    // Sysid
155    public abstract SysIdRoutine getIntakeSysIdRoutine();
156
157    public abstract void setPivotVoltageOverride(Voltage voltage);
158
159    @Override
160    public void periodic() {
161        final IntakeState currentState = getState();
162        // Logging
163        DogLog.log("Intake/State", currentState.name());
164        DogLog.forceNt.log("Intake/Pivot/Limit Switch Hit", limitSwitchHit());
165        DogLog.forceNt.log("States/Intake", currentState.name());
166        DogLog.log("Intake/Pivot/Target Angle", currentState.getTargetAngle().in(Degrees));
167        DogLog.forceNt.log("Intake/Pivot/Current Angle", getRelativePosition().in(Degrees));
168        DogLog.forceNt.log("Intake/Pivot/At Target Angle", atTargetAngle());
169        DogLog.log("Intake/Pivot/Above Threshold", isPivotAboveThreshold());
170        DogLog.forceNt.log("Intake/Rollers/Target Duty Cycle", currentState.getTargetDutyCycle());
171        DogLog.log("Intake/Rollers/RPM", getRollerVelocity().in(RPM));
172    }
173}