src/main/java/frc/robot/subsystems/Chain.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
51
52
53
54
55
56
57
58

package frc.robot.subsystems;

import org.littletonrobotics.junction.Logger;

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

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Chain extends SubsystemBase 
{
    private final CANSparkMax m_motor = new CANSparkMax(Constants.chain_ID, MotorType.kBrushed);
    private final Encoder m_encoder = new Encoder(Constants.chain_encoder_PINS[0], Constants.chain_encoder_PINS[1]);

    private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);

    public Chain()
    {
        // TODO: figure out invertedness

        // m_motor.setInverted(true);

        m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
        m_encoder.reset();
    }

    public void reset() 
    {
        m_encoder.reset();
    }

    // perc between 0 and 1;
    public void atPercentage(double perc) 
    {
        // System.out.println(perc);
        Logger.recordOutput(getName() + "/perc", perc);
        double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);
        Logger.recordOutput(getName() + "/pidOUt", pid_out);
        m_motor.setVoltage(MathUtil.clamp(pid_out, -6, 6));
    }

    public boolean isAtPosition(double pos)
    {
        if(Math.abs(pos - m_encoder.getDistance()) < Constants.ChainEpsilon)
            return true;
        return false;
    }

    @Override
    public void periodic() 
    { 
        Logger.recordOutput(getName() + "/position", m_encoder.getDistance());
    }
}