Reefscape/src/main/java/frc/robot/elevator/Elevator.java at c59e60570ecd19f0ae44995a0028afe28f3ec278 · TripleHelixProgramming/Reefscape · 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
package frc.robot.elevator;

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.InstantCommand;
import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
import frc.robot.elevator.ElevatorConstants.CoralWristConstants.CoralWristState;
import frc.robot.elevator.ElevatorConstants.LifterConstants.LifterState;

public class Elevator {

  Lifter lifter = new Lifter();
  CoralWrist coralWrist = new CoralWrist();
  CoralRoller coralRoller = new CoralRoller();
  AlgaeRoller algaeRoller = new AlgaeRoller();
  AlgaeWrist algaeWrist = new AlgaeWrist(algaeRoller.hasAlgae);

  public Elevator() {}

  public void periodic() {
    SmartDashboard.putData(lifter);
    SmartDashboard.putData(coralWrist);
    SmartDashboard.putData(coralRoller);
    SmartDashboard.putData(algaeRoller);
    SmartDashboard.putData(algaeWrist);
  }

  public Lifter getLifter() {
    return lifter;
  }

  public CoralRoller getCoralRoller() {
    return coralRoller;
  }

  public AlgaeRoller getAlgaeRoller() {
    return algaeRoller;
  }

  public CoralWrist getCoralWrist() {
    return coralWrist;
  }

  public AlgaeWrist getAlgaeWrist() {
    return algaeWrist;
  }

  public Command resetPositionControllers() {
    return new InstantCommand(
        () -> {
          lifter.matchHeight();
          lifter.resetController();
          coralWrist.resetController();
          algaeWrist.resetController();
        });
  }

  public Command coralL4PositionCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.CoralL4),
        coralWrist.createSetAngleCommand(CoralWristState.L4),
        algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
  }

  public Command coralL3PositionCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.CoralL3),
        coralWrist.createSetAngleCommand(CoralWristState.L3),
        algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
  }

  public Command coralL2PositionCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.CoralL2),
        coralWrist.createSetAngleCommand(CoralWristState.L2),
        algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
  }

  public Command coralL1PositionCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.CoralL1),
        coralWrist.createSetAngleCommand(CoralWristState.L1),
        algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
  }

  public Command coralIntakeCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.CoralIntake),
        coralWrist.createSetAngleCommand(CoralWristState.Intake),
        algaeWrist.createSetAngleCommand(AlgaeWristState.CoralMode));
  }

  public Command algaeBargePositionCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.AlgaeBarge),
        coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
        algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
  }

  public Command algaeL3IntakeCG() {
    return Commands.parallel(
            lifter.createSetHeightCommand(LifterState.AlgaeL3),
            coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
            algaeWrist.createSetAngleCommand(AlgaeWristState.L3),
            algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae))
        .andThen(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
  }

  public Command algaeL2IntakeCG() {
    return Commands.parallel(
            lifter.createSetHeightCommand(LifterState.AlgaeL2),
            coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
            algaeWrist.createSetAngleCommand(AlgaeWristState.L2),
            algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae))
        .andThen(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
  }

  public Command algaeProcessorPositionCG() {
    return Commands.parallel(
        lifter.createSetHeightCommand(LifterState.AlgaeProcessor),
        coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
        algaeWrist.createSetAngleCommand(AlgaeWristState.Processor));
  }

  public Command algaeFloorIntakeCG() {
    return Commands.parallel(
            lifter.createSetHeightCommand(LifterState.AlgaeIntakeFloor),
            coralWrist.createSetAngleCommand(CoralWristState.AlgaeMode),
            algaeWrist.createSetAngleCommand(AlgaeWristState.Floor),
            algaeRoller.createIntakeCommand().until(algaeRoller.hasAlgae))
        .andThen(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
  }
}