src/main/java/frc/robot/subsystems/Intake.java


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Intake extends SubsystemBase 
{
    private final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
    private final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
    private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();

    public Intake() 
    {
        m_intakeT.setInverted(true);
        m_intakeD.setInverted(true);
    }

    public void atPercentage(double perc) 
    {
        atPercentage(perc, perc); 
    }

    // perc is between 0.0 and 1.0
    public void atPercentage(double upPerc, double downPerc) 
    {
        m_intakeT.set(upPerc);
        m_intakeD.set(downPerc);
    }

    public void shoot() 
    {
        if (Math.abs(m_intakeD_encoder.getVelocity()) > 2000) {
            m_intakeT.set(-0.5);
        }
        m_intakeD.set(-0.5);
    }

    @Override
    public void periodic()
    {
        Logger.recordOutput(getName() + "/speedT", m_intakeT.getEncoder().getVelocity());
        Logger.recordOutput(getName() + "/speedD", m_intakeD.getEncoder().getVelocity());
    }
}