VarshaElevatorCode/Elevator.java at main · 7451AvengerRobotics/VarshaElevatorCode · 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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
package frc.robot.subsystems;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DutyCycle;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ElevatorConstants;

import static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotionMagicConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VoltageOut;

/*
 * What to do once you come to the elevator subsystem:
 * 1. Find which motor is the leader on the elevator by blinking it
 * 2. Find what direction it turns by manually moving the elevator
 * 3. Add the withInverted based on the direction it needs to turn to go up
 *
 * 4.
 */


public class Elevator extends SubsystemBase {
    private final TalonFX elevatorLeader = new TalonFX(ElevatorConstants.kElevatorLeaderID);
    private final TalonFX elevatorFollower = new TalonFX(ElevatorConstants.kElevatorFollowerID);
    private final CANcoder elevatorEncoder = new CANcoder(ElevatorConstants.kCANCoderID);
    private final DigitalInput limitSwitch = new DigitalInput(ElevatorConstants.kLimitSwitchPort);
    private final MotionMagicVoltage elevatorRequest = new MotionMagicVoltage(0);

    public Elevator() {
        TalonFXConfiguration cfg = new TalonFXConfiguration()
        .withMotorOutput(
            new MotorOutputConfigs()
                .withInverted(InvertedValue.CounterClockwise_Positive)
                .withNeutralMode(NeutralModeValue.Coast)
        ).withFeedback(
            new FeedbackConfigs()
                .withRotorToSensorRatio(1)
                .withSensorToMechanismRatio(1)
                .withFeedbackRemoteSensorID(elevatorEncoder.getDeviceID())
                .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
        ).withCurrentLimits(
            new CurrentLimitsConfigs()
                .withStatorCurrentLimit(Amps.of(40))
                .withStatorCurrentLimitEnable(true)
        ).withMotionMagic(
            new MotionMagicConfigs()
                .withMotionMagicCruiseVelocity(RotationsPerSecond.of(12))
                .withMotionMagicAcceleration(RotationsPerSecondPerSecond.of(10))
                .withMotionMagicJerk(RotationsPerSecondPerSecond.per(Second).of(0))
        ).withSlot0(
            new Slot0Configs()
                .withKP(5.0)
                .withKI(2.0)
                .withKD(0.0)
                .withKG(0.2)
                .withKS(0.0)
                .withKV(0.0)
                .withKA(0.0)
        );

        elevatorLeader.getConfigurator().apply(cfg);
        elevatorFollower.getConfigurator().apply(cfg);

        elevatorFollower.setControl(new Follower(ElevatorConstants.kElevatorLeaderID, true));

        elevatorLeader.getConfigurator().setPosition(0);
        elevatorFollower.getConfigurator().setPosition(0);
    }

    @Override
    public void periodic(){
        SmartDashboard.putNumber("Leader Rotations", elevatorLeader.getPosition().getValueAsDouble());
        SmartDashboard.putNumber("Follower Rotations", elevatorFollower.getPosition().getValueAsDouble());
        SmartDashboard.putBoolean("Limit Switch", getLimitSwitch());
    }


    public boolean getLimitSwitch() {
        return !limitSwitch.get();
    }

    public void moveMotor(double rotations) {
        elevatorLeader.setControl(elevatorRequest.withPosition(rotations));
    }

    public Command moveMotorCommand(double rotations) {
        return run(() ->
            moveMotor(rotations)
        );
    }

    public Command goLimpCommand() {
        return run(() ->
            elevatorLeader.setControl(new DutyCycleOut(0))
        );
    }

    public Command level1() {
        return moveMotorCommand(1);
    }

    public Command level2() {
        return moveMotorCommand(8);
    }

    public Command level3() {
        return moveMotorCommand(12);
    }

    public Command approachZero() {
        return moveMotorCommand(0.1);
    }

    public double getLeaderEncoderPosition() {
        return elevatorLeader.getPosition().getValueAsDouble();
    }

    public boolean closeToZero() {
        return getLeaderEncoderPosition() < 0.2;
    }

    public Command goToZero() {
        return Commands.sequence(
            moveMotorCommand(0.5).withTimeout(4),
            goLimpCommand()
        );
    }
}