5002ReefScape/Arm.java at main · dragonrobotics/5002ReefScape · GitHub

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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
package frc.robot.subsystems;

import static edu.wpi.first.wpilibj2.command.Commands.runOnce;
import static edu.wpi.first.wpilibj2.command.Commands.sequence;
import static edu.wpi.first.wpilibj2.command.Commands.waitUntil;

import java.util.function.BooleanSupplier;

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.EncoderConfig;
import com.revrobotics.spark.config.EncoderConfigAccessor;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Constants.OperatorConstants;

public class Arm extends SubsystemBase {


    final SparkMax m_armRotator = new SparkMax(OperatorConstants.m_armRotator, MotorType.kBrushless);
    final SparkMax m_armShooter = new SparkMax(OperatorConstants.m_armShooter, MotorType.kBrushless);

    SparkMaxConfig rotatorConfig = new SparkMaxConfig();
    SparkMaxConfig shooterConfig = new SparkMaxConfig();

    final RelativeEncoder encoder = m_armRotator.getEncoder();
    final AbsoluteEncoder rotatorAbsoluteEncoder = m_armRotator.getAbsoluteEncoder();

    PIDController controller = new PIDController(0.5, 0, 0);

    double factor = 0.0;

    public Arm(){
        rotatorConfig
            .idleMode(IdleMode.kCoast)
            .inverted(true)

        .encoder
            .positionConversionFactor(OperatorConstants.m_armConversionFactor)
            .velocityConversionFactor(OperatorConstants.m_armConversionFactor/60);

        shooterConfig
            .idleMode(IdleMode.kBrake);


        m_armRotator.configure(rotatorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
        m_armShooter.configure(shooterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);

        controller.setTolerance(1);
    }

    public Command moveToPosition(Double position){
        return sequence(
            runOnce(()->{controller.setSetpoint(factor);}),
            run(()->{run();}).until(atGoal())
        );
    }

    public double getMeasurement(){
        return encoder.getPosition();
    }

    public void run(){
        m_armRotator.set(controller.calculate(getMeasurement(), controller.getSetpoint()));
    }

    public void runMotor(double d){
        m_armRotator.set(d);
    }

    public void intake(){
        m_armShooter.set(-.80);
    }

    public void outake(){
        m_armShooter.set(0.8);
    }

    public BooleanSupplier atGoal(){
        return () -> controller.atSetpoint();
    }

    public void stop(){
        m_armRotator.set(0.0);
    }

    public void calibrate(){
        factor = getMeasurement();
        factor = 90 / getMeasurement();

        rotatorConfig.alternateEncoder
                .positionConversionFactor(factor)
                .velocityConversionFactor(factor/60);

    }

    public void zero(){
        encoder.setPosition(0);
    }


    @Override
    public void periodic(){
        SmartDashboard.putNumber("Arm position", getMeasurement());
        SmartDashboard.putNumber("Arm Conversion Factor", factor);
    }
}