Skip to content

Commit 2601710

Browse files
MichaelLesirgeMichaelLesirge
committed
operator improvements + hang
1 parent 5076098 commit 2601710

File tree

10 files changed

+130
-189
lines changed

10 files changed

+130
-189
lines changed

‎elastic-layout.json‎

Lines changed: 20 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -185,27 +185,10 @@
185185
"false_icon": "None"
186186
}
187187
},
188-
{
189-
"title": "Intake Sensor?",
190-
"x": 704.0,
191-
"y": 0.0,
192-
"width": 128.0,
193-
"height": 128.0,
194-
"type": "Boolean Box",
195-
"properties": {
196-
"topic": "/SmartDashboard/Intake Sensor?",
197-
"period": 0.06,
198-
"data_type": "boolean",
199-
"true_color": 4283215696,
200-
"false_color": 4294198070,
201-
"true_icon": "None",
202-
"false_icon": "None"
203-
}
204-
},
205188
{
206189
"title": "Hang Value",
207190
"x": 832.0,
208-
"y": 128.0,
191+
"y": 0.0,
209192
"width": 128.0,
210193
"height": 128.0,
211194
"type": "Text Display",
@@ -218,7 +201,7 @@
218201
},
219202
{
220203
"title": "At Goal?",
221-
"x": 832.0,
204+
"x": 704.0,
222205
"y": 0.0,
223206
"width": 128.0,
224207
"height": 128.0,
@@ -243,7 +226,7 @@
243226
"properties": {
244227
"topic": "/SmartDashboard/Field",
245228
"period": 0.06,
246-
"field_game": "Reefscape",
229+
"field_game": "Crescendo",
247230
"robot_width": 0.85,
248231
"robot_length": 0.85,
249232
"show_other_objects": true,
@@ -254,16 +237,30 @@
254237
}
255238
},
256239
{
257-
"title": "Hang",
240+
"title": "Coral Wrist Degrees",
241+
"x": 832.0,
242+
"y": 128.0,
243+
"width": 128.0,
244+
"height": 128.0,
245+
"type": "Text Display",
246+
"properties": {
247+
"topic": "/SmartDashboard/Coral Wrist Degrees",
248+
"period": 0.06,
249+
"data_type": "double",
250+
"show_submit_button": false
251+
}
252+
},
253+
{
254+
"title": "Elevator Height Meters",
258255
"x": 832.0,
259256
"y": 256.0,
260257
"width": 128.0,
261258
"height": 128.0,
262259
"type": "Text Display",
263260
"properties": {
264-
"topic": "/SmartDashboard/Hang",
261+
"topic": "/SmartDashboard/Elevator Height Meters",
265262
"period": 0.06,
266-
"data_type": "string",
263+
"data_type": "double",
267264
"show_submit_button": false
268265
}
269266
}

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

Lines changed: 18 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -277,7 +277,7 @@ private void initDashboard() {
277277

278278
dashboard.setSensorSuppliers(
279279
coralIntake::usingSensor, () -> coralIntake.hasCoral().orElse(false));
280-
dashboard.setHangSuppliers(hang::getRawValue, hang::withinSafeToleranceSoftLimits);
280+
dashboard.setHangSuppliers(hang::getMeasuredPosition);
281281
dashboard.setSuperstructureAtGoal(superstructure::atGoal);
282282

283283
dashboard.addCommand("Reset Pose", () -> drive.resetPose(new Pose2d()), true);
@@ -313,7 +313,7 @@ public void updateAlerts() {
313313
private void configureControllerBindings() {
314314
CommandScheduler.getInstance().getActiveButtonLoop().clear();
315315
configureDriverControllerBindings(driverController, true);
316-
configureOperatorControllerBindings(operatorController, false);
316+
configureOperatorControllerBindings(operatorController, true);
317317
configureAlertTriggers();
318318
}
319319

@@ -472,9 +472,9 @@ private void configureOperatorControllerBindings(
472472
coralWrist.setSlowModeSupplier(() -> coralIntake.hasCoral().orElse(false));
473473

474474
new Trigger(() -> coralIntake.hasCoral().orElse(false))
475-
.onChange(rumbleControllers(0.5).withTimeout(0.1));
475+
.onChange(rumbleControllers(0.8).withTimeout(0.15));
476476

477-
// Intake and score
477+
// Main superstructure control
478478

479479
final double heightOffsetAdjustment = Units.inchesToMeters(1);
480480
final Rotation2d angleOffsetAdjustment = Rotation2d.fromDegrees(5);
@@ -485,9 +485,8 @@ private void configureOperatorControllerBindings(
485485
if (level.equals(Superstructure.State.L1)) {
486486
trigger.whileTrue(superstructure.run(level));
487487
} else {
488-
trigger.whileTrue(superstructure.run2(level));
488+
trigger.whileTrue(superstructure.runSequenced(level));
489489
}
490-
// trigger.whileTrue(superstructure.run(level));
491490
} else if (level.isIntake()) {
492491
trigger.whileTrue(superstructure.run(level));
493492
}
@@ -498,6 +497,7 @@ private void configureOperatorControllerBindings(
498497
.and(superstructure::atGoal)
499498
.whileTrue(superstructure.runWheels(level))
500499
.onTrue(Commands.runOnce(sensor::simulateItemEjection));
500+
trigger.and(xbox.rightBumper().whileTrue(superstructure.runWheels(level)));
501501
} else if (level.isIntake()) {
502502
trigger.whileTrue(
503503
superstructure.runWheels(level).until(() -> coralIntake.hasCoral().orElse(false)));
@@ -526,24 +526,23 @@ private void configureOperatorControllerBindings(
526526

527527
final Trigger anyButton = xbox.a().or(xbox.x()).or(xbox.y()).or(xbox.b());
528528

529+
final Trigger algae = xbox.leftBumper();
529530
configureOperatorControllerBindingLevel.accept(xbox.y(), Superstructure.State.L4);
530-
configureOperatorControllerBindingLevel.accept(xbox.x(), Superstructure.State.L3);
531-
configureOperatorControllerBindingLevel.accept(xbox.a(), Superstructure.State.L2);
531+
configureOperatorControllerBindingLevel.accept(xbox.x().and(algae.negate()), Superstructure.State.L3);
532+
configureOperatorControllerBindingLevel.accept(xbox.a().and(algae.negate()), Superstructure.State.L2);
532533
configureOperatorControllerBindingLevel.accept(xbox.b(), Superstructure.State.L1);
534+
configureOperatorControllerBindingLevel.accept(xbox.x().and(algae), Superstructure.State.L3_ALGAE);
535+
configureOperatorControllerBindingLevel.accept(xbox.a().and(algae), Superstructure.State.L2_ALGAE);
533536

534-
final double intakeManualSpeed = 0.24;
535-
anyButton.and(xbox.rightBumper()).whileTrue(coralIntake.runMotors(-intakeManualSpeed));
536-
anyButton.and(xbox.leftBumper()).whileTrue(coralIntake.runMotors(+intakeManualSpeed));
537+
xbox.leftStick()
538+
.onTrue(superstructure.run(Superstructure.State.STOW_HIGH));
537539

538540
xbox.rightStick()
539-
.onTrue(
540-
Commands.parallel(
541-
Commands.runOnce(() -> elevator.setGoalHeightMeters(State.STOW_HIGH.getHeight())),
542-
Commands.runOnce(() -> coralWrist.setGoalRotation(State.STOW_HIGH.getAngle()))));
541+
.onTrue(superstructure.run(Superstructure.State.STOW_LOW));
543542

544543
// Intake
545544

546-
coralIntake.setDefaultCommand(superstructure.passiveIntake());
545+
coralIntake.setDefaultCommand(superstructure.stopWheels());
547546

548547
xbox.start()
549548
.debounce(0.3)
@@ -576,9 +575,9 @@ private void configureOperatorControllerBindings(
576575
-1,
577576
+1);
578577

579-
// new Trigger(() -> hangSpeed.getAsDouble() != 0)
580-
// .and(DriverStation::isTeleopEnabled)
581-
// .whileTrue(hang.run(() -> hang.set(hangSpeed.getAsDouble())).finallyDo(hang::stop));
578+
new Trigger(() -> hangSpeed.getAsDouble() != 0)
579+
.and(DriverStation::isTeleopEnabled)
580+
.whileTrue(hang.run(() -> hang.set(hangSpeed.getAsDouble())).finallyDo(hang::stop));
582581

583582
Function<RumbleType, Command> rumble =
584583
(rumbleType) ->
@@ -587,7 +586,6 @@ private void configureOperatorControllerBindings(
587586
.withTimeout(0.2);
588587

589588
if (hangSetpoints) {
590-
// TODO for district comps!
591589
xbox.rightBumper()
592590
.debounce(0.1)
593591
.and(xbox.leftBumper().negate().debounce(0.1))

‎src/main/java/frc/robot/subsystems/dashboard/DriverDashboard.java‎

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,6 @@ public static DriverDashboard getInstance() {
5151

5252
private BooleanSupplier superstructureAtGoal;
5353

54-
private BooleanSupplier hangWithinTolerance;
5554
private DoubleSupplier hangValue;
5655

5756
private final Field2d field = new Field2d();
@@ -99,9 +98,8 @@ public void setSensorSuppliers(BooleanSupplier usingIntakeSensor, BooleanSupplie
9998
this.hasCoral = hasCoral;
10099
}
101100

102-
public void setHangSuppliers(DoubleSupplier hangValue, BooleanSupplier hangWithTolerance) {
101+
public void setHangSuppliers(DoubleSupplier hangValue) {
103102
this.hangValue = hangValue;
104-
this.hangWithinTolerance = hangWithTolerance;
105103
}
106104

107105
public void setSuperstructureAtGoal(BooleanSupplier superstructureAtGoal) {
@@ -173,9 +171,5 @@ public void periodic() {
173171
if (hangValue != null) {
174172
SmartDashboard.putNumber("Hang Value", hangValue.getAsDouble());
175173
}
176-
177-
if (hangWithinTolerance != null) {
178-
SmartDashboard.putBoolean("Hang Good?", hangWithinTolerance.getAsBoolean());
179-
}
180174
}
181175
}

‎src/main/java/frc/robot/subsystems/hang/Hang.java‎

Lines changed: 28 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
package frc.robot.subsystems.hang;
22

3+
import edu.wpi.first.math.controller.PIDController;
34
import edu.wpi.first.math.geometry.Rotation2d;
4-
import edu.wpi.first.math.util.Units;
55
import edu.wpi.first.wpilibj.Alert;
66
import edu.wpi.first.wpilibj.Alert.AlertType;
77
import edu.wpi.first.wpilibj.util.Color;
@@ -26,90 +26,80 @@ public class Hang extends SubsystemBase {
2626
private static final LoggedTunableNumber kD =
2727
hangFactory.getNumber("kD", HangConstants.FEEDBACK.kD());
2828

29-
private HangVisualization measuredVisualizer = new HangVisualization("Measured", Color.kYellow);
30-
private HangVisualization setpointVisualizer = new HangVisualization("Goal", Color.kGreen);
29+
private final HangVisualization measuredVisualizer =
30+
new HangVisualization("Measured", Color.kYellow);
31+
private final HangVisualization setpointVisualizer = new HangVisualization("Goal", Color.kGreen);
3132

3233
private final Alert motorConnectedAlert = new Alert("Hang Motor Disconnected", AlertType.kError);
3334

34-
private Rotation2d lastGoal = Rotation2d.kZero;
35+
private final PIDController controller = new PIDController(0.0, 0.0, 0.0);
36+
private boolean runClosedLoop = false;
3537

3638
public Hang(HangIO io) {
3739
this.io = io;
38-
39-
io.setPID(kP.get(), kI.get(), kD.get());
4040
io.setBrakeMode(true);
41+
io.setLimits(HangConstants.RELATIVE_MIN_ROTATIONS, HangConstants.RELATIVE_MAX_ROTATIONS);
4142
}
4243

4344
@Override
4445
public void periodic() {
4546
io.updateInputs(inputs);
4647
Logger.processInputs("Hang", inputs);
4748

49+
Logger.recordOutput("Hang/RunClosedLoop", runClosedLoop);
50+
51+
if (runClosedLoop) {
52+
io.runOpenLoop(controller.calculate(inputs.positionRotations));
53+
}
54+
4855
motorConnectedAlert.set(!inputs.motorConnected);
4956

5057
LoggedTunableNumber.ifChanged(
51-
hashCode(), (values) -> io.setPID(values[0], values[1], values[2]), kP, kI, kD);
58+
hashCode(), (values) -> controller.setPID(values[0], values[1], values[2]), kP, kI, kD);
5259

5360
measuredVisualizer.update(Rotation2d.fromRotations(inputs.positionRotations));
5461
}
5562

56-
private void setLastGoal(Rotation2d goal) {
57-
io.runPosition(goal.getRotations());
58-
lastGoal = goal;
59-
setpointVisualizer.update(goal);
60-
}
61-
62-
public Rotation2d getMeasuredPosition() {
63-
return Rotation2d.fromRotations(inputs.positionRotations);
64-
}
65-
66-
public Rotation2d getLastGoal() {
67-
return lastGoal;
68-
}
69-
70-
@AutoLogOutput(key = "Hang/MeasuredPositionDegrees")
71-
public double getMeasuredPositionDegrees() {
72-
return Units.rotationsToDegrees(inputs.positionRotations);
73-
}
74-
75-
@AutoLogOutput(key = "Hang/LastGoalDegrees")
76-
public double getLastGoalDegrees() {
77-
return lastGoal.getDegrees();
63+
private void setGoal(double rotations) {
64+
runClosedLoop = true;
65+
controller.setSetpoint(rotations);
66+
setpointVisualizer.update(Rotation2d.fromRotations(rotations));
7867
}
7968

80-
// TEMP FOR QUICK FIX
81-
public double getRawValue() {
69+
@AutoLogOutput(key = "Hang/MeasuredPositionRotations")
70+
public double getMeasuredPosition() {
8271
return inputs.positionRotations;
8372
}
8473

85-
// TEMP FOR QUICK FIX
86-
public boolean withinSafeToleranceSoftLimits() {
87-
return inputs.positionRotations > HangConstants.MIN_SAFE_RAW_VALUE_SOFT_LIMIT
88-
&& inputs.positionRotations < HangConstants.MAX_SAFE_RAW_VALUE_SOFT_LIMIT;
74+
@AutoLogOutput(key = "Hang/LastGoalPositionRotations")
75+
public double getGoal() {
76+
return controller.getSetpoint();
8977
}
9078

9179
public Command stow() {
92-
return runOnce(() -> setLastGoal(HangConstants.STOWED_POSITION_ROTATIONS));
80+
return runOnce(() -> setGoal(HangConstants.STOWED_POSITION_ROTATIONS));
9381
}
9482

9583
public Command deploy() {
96-
return runOnce(() -> setLastGoal(HangConstants.DEPLOY_POSITION_ROTATIONS));
84+
return runOnce(() -> setGoal(HangConstants.DEPLOY_POSITION_ROTATIONS));
9785
}
9886

9987
public Command retract() {
100-
return runOnce(() -> setLastGoal(HangConstants.RETRACT_POSITION_ROTATIONS));
88+
return runOnce(() -> setGoal(HangConstants.RETRACT_POSITION_ROTATIONS));
10189
}
10290

10391
public Command runSet(double speed) {
10492
return runEnd(() -> io.runOpenLoop(speed), io::stop);
10593
}
10694

10795
public void set(double speed) {
96+
runClosedLoop = false;
10897
io.runOpenLoop(speed);
10998
}
11099

111100
/** Stop the arm from moving. */
112101
public void stop() {
102+
runClosedLoop = false;
113103
io.stop();
114104
}
115105
}

‎src/main/java/frc/robot/subsystems/hang/HangConstants.java‎

Lines changed: 8 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
package frc.robot.subsystems.hang;
22

3-
import edu.wpi.first.math.geometry.Rotation2d;
43
import edu.wpi.first.math.util.Units;
54
import frc.robot.Constants;
65
import frc.robot.utility.records.PIDConstants;
@@ -13,26 +12,26 @@ public static record HangConfig(
1312

1413
public static final HangConfig HANG_CONFIG =
1514
switch (Constants.getRobot()) {
16-
case COMP_BOT_2025 -> new HangConfig(16, 0.304, false, false);
15+
case COMP_BOT_2025 -> new HangConfig(16, 0.304, true, false);
1716
default -> new HangConfig(0, 0.0, false, false);
1817
};
1918

2019
public static final PIDConstants FEEDBACK =
2120
switch (Constants.getRobot()) {
22-
case COMP_BOT_2025 -> new PIDConstants(1.0, 0.0, 0.0);
21+
case COMP_BOT_2025 -> new PIDConstants(10.0, 0.0, 0.0);
2322
case SIM_BOT -> new PIDConstants(20.0, 0.0, 0.0);
2423
default -> new PIDConstants(1.0, 0.0, 0.0);
2524
};
2625

2726
public static final int MOTOR_CURRENT_LIMIT = 40;
2827
public static final double GEAR_REDUCTION = 5 * 5 * 5;
2928

30-
public static final Rotation2d STOWED_POSITION_ROTATIONS = Rotation2d.fromDegrees(0);
31-
public static final Rotation2d DEPLOY_POSITION_ROTATIONS = Rotation2d.fromDegrees(60);
32-
public static final Rotation2d RETRACT_POSITION_ROTATIONS = Rotation2d.fromDegrees(-30);
29+
public static final double STOWED_POSITION_ROTATIONS = 0.0;
30+
public static final double DEPLOY_POSITION_ROTATIONS = 0.0;
31+
public static final double RETRACT_POSITION_ROTATIONS = 0.0;
3332

34-
public static final double TOLERANCE = Units.degreesToRotations(0.3);
33+
public static final double RELATIVE_MIN_ROTATIONS = -1;
34+
public static final double RELATIVE_MAX_ROTATIONS = +1;
3535

36-
public static final double MIN_SAFE_RAW_VALUE_SOFT_LIMIT = -1;
37-
public static final double MAX_SAFE_RAW_VALUE_SOFT_LIMIT = +1;
36+
public static final double TOLERANCE = Units.degreesToRotations(0.3);
3837
}

‎src/main/java/frc/robot/subsystems/hang/HangIO.java‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -19,14 +19,10 @@ public static class HangIOInputs {
1919

2020
public default void updateInputs(HangIOInputs inputs) {}
2121

22-
public default void runPosition(double positionRotations) {}
23-
2422
public default void runOpenLoop(double output) {}
2523

2624
public default void runVolts(double volts) {}
2725

28-
public default void setPID(double kP, double kI, double kD) {}
29-
3026
public default void setBrakeMode(boolean enable) {}
3127

3228
public default void stop() {}

0 commit comments

Comments
 (0)