Skip to content

Commit 908d96a

Browse files
MichaelLesirgeMichaelLesirge
committed
fine control of superstructure
1 parent d8c859c commit 908d96a

File tree

4 files changed

+96
-116
lines changed

4 files changed

+96
-116
lines changed

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 38 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -480,29 +480,45 @@ private void configureOperatorControllerBindings(CommandXboxController xbox) {
480480
new Trigger(DriverStation::isEnabled)
481481
.onTrue(superstructure.runAction(Superstructure.State.STOW_HIGH));
482482

483-
xbox.back().onTrue(drive.runOnce(drive::stop).withName("CANCEL and stop"));
484-
485-
// Primary scoring
486-
xbox.leftTrigger()
487-
.whileTrue(superstructure.run(State.INTAKE).alongWith(superstructure.intake()));
483+
final double heightOffsetAdjustment = Units.inchesToMeters(1);
484+
final Rotation2d angleOffsetAdjustment = Rotation2d.fromDegrees(5);
488485

489486
final BiConsumer<Trigger, Superstructure.State> configureOperatorControllerBindingLevel =
490487
(trigger, level) -> {
491-
trigger.whileTrue(
492-
superstructure
493-
.run(level)
494-
.alongWith(
495-
Commands.waitUntil(superstructure::atGoal)
496-
.andThen(superstructure.runWheels(level).asProxy())));
488+
trigger.whileTrue(superstructure.run(level));
489+
490+
if (level.isLevel()) {
491+
trigger
492+
.and(xbox.rightTrigger())
493+
.and(superstructure::atGoal)
494+
.whileTrue(superstructure.runWheels(level));
495+
} else {
496+
trigger.whileTrue(superstructure.runWheels(level));
497+
}
498+
499+
trigger
500+
.and(xbox.povUp())
501+
.onTrue(Commands.runOnce(() -> level.adjustHeight(heightOffsetAdjustment)));
502+
trigger
503+
.and(xbox.povDown())
504+
.onTrue(Commands.runOnce(() -> level.adjustHeight(-heightOffsetAdjustment)));
505+
trigger
506+
.and(xbox.povRight())
507+
.onTrue(Commands.runOnce(() -> level.adjustAngle(angleOffsetAdjustment)));
508+
trigger
509+
.and(xbox.povLeft())
510+
.onTrue(
511+
Commands.runOnce(() -> level.adjustAngle(angleOffsetAdjustment.unaryMinus())));
512+
trigger.and(xbox.back()).onTrue(Commands.runOnce(level::adjustReset));
497513
};
498514

515+
configureOperatorControllerBindingLevel.accept(xbox.leftTrigger(), Superstructure.State.INTAKE);
516+
499517
configureOperatorControllerBindingLevel.accept(xbox.y(), Superstructure.State.L4);
500518
configureOperatorControllerBindingLevel.accept(xbox.x(), Superstructure.State.L3);
501519
configureOperatorControllerBindingLevel.accept(xbox.a(), Superstructure.State.L2);
502520
configureOperatorControllerBindingLevel.accept(xbox.b(), Superstructure.State.L1);
503521

504-
xbox.povDown().onTrue(superstructure.stowLow());
505-
506522
// Intake
507523

508524
coralIntake.setDefaultCommand(superstructure.passiveIntake());
@@ -514,6 +530,7 @@ private void configureOperatorControllerBindings(CommandXboxController xbox) {
514530
+ JoystickUtil.applyDeadband(-xbox.getLeftY()),
515531
-1,
516532
+1);
533+
517534
new Trigger(() -> intakeSpeed.getAsDouble() != 0)
518535
.and(DriverStation::isTeleopEnabled)
519536
.whileTrue(coralIntake.run(() -> coralIntake.setMotors(intakeSpeed.getAsDouble())));
@@ -560,24 +577,24 @@ private void registerNamedCommands() {
560577
NamedCommands.registerCommand(
561578
"l1",
562579
Commands.parallel(
563-
Commands.runOnce(() -> elevator.setGoalHeightMeters(Superstructure.L2_HEIGHT)),
564-
Commands.runOnce(() -> coralWrist.setGoalRotation(Superstructure.L2_CORAL_ANGLE)))
565-
.andThen(Commands.waitSeconds(2))
580+
Commands.runOnce(() -> elevator.setGoalHeightMeters(State.L1.getHeight())),
581+
Commands.runOnce(() -> coralWrist.setGoalRotation(State.L1.getAngle())))
582+
.andThen(Commands.waitUntil(superstructure::atGoal))
566583
.andThen(
567584
Commands.runEnd(() -> coralIntake.setMotors(1), coralIntake::stopMotors)
568585
.withTimeout(0.5)));
569586

570587
NamedCommands.registerCommand(
571588
"stow",
572589
Commands.parallel(
573-
Commands.runOnce(() -> elevator.setGoalHeightMeters(Superstructure.STOW_HEIGHT)),
574-
Commands.runOnce(() -> coralWrist.setGoalRotation(Superstructure.STOW_CORAL_ANGLE))));
590+
Commands.runOnce(() -> elevator.setGoalHeightMeters(State.STOW.getHeight())),
591+
Commands.runOnce(() -> coralWrist.setGoalRotation(State.STOW.getAngle()))));
592+
575593
NamedCommands.registerCommand(
576594
"intake",
577595
Commands.parallel(
578-
Commands.runOnce(() -> elevator.setGoalHeightMeters(Superstructure.INTAKE_HEIGHT)),
579-
Commands.runOnce(
580-
() -> coralWrist.setGoalRotation(Superstructure.INTAKE_CORAL_ANGLE)))
596+
Commands.runOnce(() -> elevator.setGoalHeightMeters(State.INTAKE.getHeight())),
597+
Commands.runOnce(() -> coralWrist.setGoalRotation(State.INTAKE.getAngle())))
581598
.andThen(
582599
Commands.runEnd(
583600
() -> coralIntake.setMotors(-0.6), () -> coralIntake.setMotors(0.05)))

‎src/main/java/frc/robot/subsystems/superstructure/Superstructure.java‎

Lines changed: 43 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -18,42 +18,57 @@ public class Superstructure extends VirtualSubsystem {
1818
private final Intake coralIntake;
1919

2020
public static enum State {
21-
STOW,
22-
STOW_HIGH,
23-
INTAKE,
21+
STOW(0.054886473109919, -90),
22+
STOW_HIGH(0, 90),
23+
INTAKE(0.218 + Units.inchesToMeters(1.5), 35),
2424

25-
L1(false),
26-
L2(false),
27-
L3(false),
28-
L4(false);
25+
L1(0.151148670108898, 0),
26+
L2(0.469491383230037 + Units.inchesToMeters(3), -35),
27+
L3(1.035 + Units.inchesToMeters(2), -35),
28+
L4(L3.height, L3.angle.getDegrees());
2929

30-
// L2_ALGAE(true),
31-
// L3_ALGAE(true);
30+
private final double height;
31+
private final Rotation2d angle;
32+
33+
private double offsetHeight = 0;
34+
private Rotation2d offsetAngle = Rotation2d.kZero;
35+
36+
private State(double height, double angleDegrees) {
37+
this.height = height;
38+
this.angle = Rotation2d.fromDegrees(angleDegrees);
39+
}
40+
41+
public void adjustHeight(double offset) {
42+
offsetHeight += offset;
43+
}
44+
45+
public void adjustAngle(Rotation2d offset) {
46+
offsetAngle = offsetAngle.plus(offset);
47+
}
48+
49+
public void adjustReset() {
50+
offsetHeight = 0;
51+
offsetAngle = Rotation2d.kZero;
52+
}
3253

3354
public boolean isLevel() {
3455
return this == L1 || this == L2 || this == L3 || this == L4;
3556
}
3657

37-
public boolean isAlgae() {
38-
return algae;
58+
public boolean isIntake() {
59+
return this == INTAKE;
3960
}
4061

41-
public State asAlgae() {
42-
return switch (this) {
43-
// case L2 -> L2_ALGAE;
44-
// case L3 -> L3_ALGAE;
45-
default -> this;
46-
};
62+
public boolean isStow() {
63+
return this == STOW || this == STOW_HIGH;
4764
}
4865

49-
public boolean algae;
50-
51-
private State() {
52-
this.algae = false;
66+
public double getHeight() {
67+
return height + offsetHeight;
5368
}
5469

55-
private State(boolean algae) {
56-
this.algae = algae;
70+
public Rotation2d getAngle() {
71+
return angle.plus(offsetAngle);
5772
}
5873
}
5974

@@ -81,15 +96,9 @@ public Command runAction(State newGoal) {
8196
}
8297

8398
public Command runPrepare(State goal) {
84-
return switch (goal) {
85-
case STOW -> stowLow();
86-
case STOW_HIGH -> stowHigh();
87-
case L1 -> prepareL1();
88-
case L2 -> prepareL2();
89-
case L3 -> prepareL3();
90-
case L4 -> prepareL4();
91-
case INTAKE -> prepareIntake();
92-
};
99+
return Commands.parallel(
100+
elevator.runPositionPrepare(goal::getHeight),
101+
coralWrist.runPositionPrepare(goal::getAngle));
93102
}
94103

95104
public Command runWheels(State goal) {
@@ -104,64 +113,6 @@ public Command runWheels(State goal) {
104113
};
105114
}
106115

107-
public static final double L1_HEIGHT = 0.151148670108898;
108-
public static final Rotation2d L1_CORAL_ANGLE = Rotation2d.fromDegrees(0);
109-
110-
public static final double L2_HEIGHT = 0.469491383230037 + Units.inchesToMeters(3);
111-
public static final Rotation2d L2_CORAL_ANGLE = Rotation2d.fromDegrees(-35);
112-
113-
public static final double L3_HEIGHT = 1.035 + Units.inchesToMeters(2);
114-
public static final Rotation2d L3_CORAL_ANGLE = Rotation2d.fromDegrees(-35);
115-
116-
public static final double L4_HEIGHT = L3_HEIGHT;
117-
public static final Rotation2d L4_CORAL_ANGLE = L3_CORAL_ANGLE;
118-
119-
public static final double INTAKE_HEIGHT = 0.218 + Units.inchesToMeters(1.5);
120-
public static final Rotation2d INTAKE_CORAL_ANGLE = Rotation2d.fromDegrees(35);
121-
122-
public static final double STOW_HEIGHT = 0.054886473109919;
123-
public static final Rotation2d STOW_CORAL_ANGLE = Rotation2d.fromDegrees(-90);
124-
125-
public static final double STOW_HEIGHT_HIGH = 0;
126-
public static final Rotation2d STOW_CORAL_ANGLE_HIGH = Rotation2d.fromDegrees(90);
127-
128-
public Command prepareL1() {
129-
return Commands.parallel(
130-
elevator.runPositionPrepare(L1_HEIGHT), coralWrist.runPositionPrepare(L1_CORAL_ANGLE));
131-
}
132-
133-
public Command prepareL2() {
134-
return Commands.parallel(
135-
elevator.runPositionPrepare(L2_HEIGHT), coralWrist.runPositionPrepare(L2_CORAL_ANGLE));
136-
}
137-
138-
public Command prepareL3() {
139-
return Commands.parallel(
140-
elevator.runPositionPrepare(L3_HEIGHT), coralWrist.runPositionPrepare(L3_CORAL_ANGLE));
141-
}
142-
143-
public Command prepareL4() {
144-
return Commands.parallel(
145-
elevator.runPositionPrepare(L4_HEIGHT), coralWrist.runPositionPrepare(L4_CORAL_ANGLE));
146-
}
147-
148-
public Command prepareIntake() {
149-
return Commands.parallel(
150-
elevator.runPositionPrepare(INTAKE_HEIGHT),
151-
coralWrist.runPositionPrepare(INTAKE_CORAL_ANGLE));
152-
}
153-
154-
public Command stowLow() {
155-
return Commands.parallel(
156-
elevator.runPositionPrepare(STOW_HEIGHT), coralWrist.runPositionPrepare(STOW_CORAL_ANGLE));
157-
}
158-
159-
public Command stowHigh() {
160-
return Commands.parallel(
161-
elevator.runPositionPrepare(STOW_HEIGHT_HIGH),
162-
coralWrist.runPositionPrepare(STOW_CORAL_ANGLE_HIGH));
163-
}
164-
165116
public Command intake() {
166117
return coralIntake.runMotors(-0.6);
167118
}
@@ -187,8 +138,8 @@ public boolean atGoal() {
187138
}
188139

189140
public void setPositionStow() {
190-
elevator.setGoalHeightMeters(STOW_HEIGHT_HIGH);
191-
coralWrist.setGoalRotation(STOW_CORAL_ANGLE_HIGH);
141+
elevator.setGoalHeightMeters(State.STOW_HIGH.height);
142+
coralWrist.setGoalRotation(State.STOW_HIGH.angle);
192143
}
193144

194145
@Override

‎src/main/java/frc/robot/subsystems/superstructure/elevator/Elevator.java‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import frc.robot.Constants;
1818
import frc.robot.utility.tunable.LoggedTunableNumber;
1919
import frc.robot.utility.tunable.LoggedTunableNumberFactory;
20+
import java.util.function.DoubleSupplier;
2021
import java.util.function.Supplier;
2122
import org.littletonrobotics.junction.AutoLogOutput;
2223
import org.littletonrobotics.junction.Logger;
@@ -171,10 +172,18 @@ public void periodic() {
171172
zeroingAlert.set(!zeroedHeightEncoder);
172173
}
173174

175+
// --- Commands ---
176+
174177
public Command runPositionPrepare(double position) {
175178
return runOnce(() -> setGoalHeightMeters(position));
176179
}
177180

181+
public Command runPositionPrepare(DoubleSupplier position) {
182+
return runOnce(() -> setGoalSupplier(() -> new State(position.getAsDouble(), 0)));
183+
}
184+
185+
// --- Methods ---
186+
178187
public void setGoalSupplier(Supplier<State> goal) {
179188
this.goalSupplier = goal;
180189
}

‎src/main/java/frc/robot/subsystems/superstructure/wrist/Wrist.java‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
import edu.wpi.first.wpilibj.Alert;
1313
import edu.wpi.first.wpilibj.DriverStation;
1414
import edu.wpi.first.wpilibj2.command.Command;
15-
import edu.wpi.first.wpilibj2.command.Commands;
1615
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1716
import frc.robot.Constants;
1817
import frc.robot.utility.tunable.LoggedTunableNumber;
@@ -138,14 +137,18 @@ public void periodic() {
138137
motorConnectedAlert.set(!inputs.motorConnected);
139138
}
140139

140+
// --- Commands ---
141+
141142
public Command runPositionPrepare(Rotation2d position) {
142143
return runOnce(() -> setGoalRotation(position));
143144
}
144145

145-
public Command runPosition(Rotation2d position) {
146-
return runPositionPrepare(position).andThen(Commands.waitUntil(this::atGoal));
146+
public Command runPositionPrepare(Supplier<Rotation2d> position) {
147+
return runOnce(() -> setGoalSupplier(() -> new State(position.get().getRadians(), 0)));
147148
}
148149

150+
// --- Methods ---
151+
149152
public void setGoalRotation(Rotation2d position) {
150153
setGoalSupplier(() -> new State(position.getRadians(), 0));
151154
}

0 commit comments

Comments
 (0)