Skip to content

Commit ea88276

Browse files
RobotLeopard86RobotLeopard86
authored
Merge branch 'main' into addressable-led
2 parents ed4e9b5 + d8c859c commit ea88276

19 files changed

+403
-94
lines changed

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

Lines changed: 44 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
import frc.robot.Constants.Mode;
2626
import frc.robot.commands.AdaptiveAutoAlignCommands;
2727
import frc.robot.commands.DriveCommands;
28+
import frc.robot.commands.ManualAlignCommands;
2829
import frc.robot.commands.controllers.JoystickInputController;
2930
import frc.robot.commands.controllers.SpeedLevelController;
3031
import frc.robot.subsystems.addressableled.AddressableLEDSubsystem;
@@ -64,9 +65,11 @@
6465
import frc.robot.subsystems.vision.CameraIOPhotonVision;
6566
import frc.robot.subsystems.vision.CameraIOSim;
6667
import frc.robot.subsystems.vision.VisionConstants;
68+
import frc.robot.utility.JoystickUtil;
6769
import frc.robot.utility.OverrideSwitch;
6870
import frc.robot.utility.commands.CustomCommands;
6971
import java.util.Arrays;
72+
import java.util.function.BiConsumer;
7073
import java.util.function.DoubleSupplier;
7174
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
7275

@@ -413,6 +416,18 @@ private void configureDriverControllerBindings(
413416
.ignoringDisable(true)
414417
.withName("Reset Gyro Heading"));
415418

419+
final BiConsumer<Trigger, Command> configureAlignmentAuto =
420+
(trigger, command) -> {
421+
trigger.onTrue(command.until(() -> input.getOmegaRadiansPerSecond() != 0));
422+
};
423+
424+
configureAlignmentAuto.accept(
425+
xbox.povRight(), ManualAlignCommands.alignToSourceRight(drive, input));
426+
configureAlignmentAuto.accept(
427+
xbox.povLeft(), ManualAlignCommands.alignToSourceLeft(drive, input));
428+
configureAlignmentAuto.accept(xbox.povDown(), ManualAlignCommands.alignToCageAdv(drive, input));
429+
configureAlignmentAuto.accept(xbox.povUp(), ManualAlignCommands.alignToReef(drive, input));
430+
416431
if (includeAutoAlign) {
417432
// Align to reef
418433

@@ -479,48 +494,54 @@ private void configureDriverControllerBindings(
479494
private void configureOperatorControllerBindings(CommandXboxController xbox) {
480495

481496
new Trigger(DriverStation::isEnabled)
482-
.onTrue(
483-
Commands.runOnce(() -> coralWrist.setConstraints(3, 8))
484-
.andThen(superstructure.runAction(Superstructure.State.STOW))
485-
.finallyDo(coralWrist::resetContraints));
497+
.onTrue(superstructure.runAction(Superstructure.State.STOW_HIGH));
486498

487499
xbox.back().onTrue(drive.runOnce(drive::stop).withName("CANCEL and stop"));
488500

489501
// Primary scoring
490502
xbox.leftTrigger()
491503
.whileTrue(superstructure.run(State.INTAKE).alongWith(superstructure.intake()));
492504

493-
xbox.rightTrigger().and(xbox.a().negate()).whileTrue(superstructure.outtake());
494-
xbox.rightTrigger().and(xbox.a()).whileTrue(superstructure.outtakeL1());
495-
496-
configureOperatorControllerBindingLevel(xbox.y(), Superstructure.State.L4);
497-
configureOperatorControllerBindingLevel(xbox.x(), Superstructure.State.L3);
498-
configureOperatorControllerBindingLevel(xbox.a(), Superstructure.State.L2);
499-
500-
configureOperatorControllerBindingLevel(xbox.b(), Superstructure.State.L1);
505+
final BiConsumer<Trigger, Superstructure.State> configureOperatorControllerBindingLevel =
506+
(trigger, level) -> {
507+
trigger.whileTrue(
508+
superstructure
509+
.run(level)
510+
.alongWith(
511+
Commands.waitUntil(superstructure::atGoal)
512+
.andThen(superstructure.runWheels(level).asProxy())));
513+
};
514+
515+
configureOperatorControllerBindingLevel.accept(xbox.y(), Superstructure.State.L4);
516+
configureOperatorControllerBindingLevel.accept(xbox.x(), Superstructure.State.L3);
517+
configureOperatorControllerBindingLevel.accept(xbox.a(), Superstructure.State.L2);
518+
configureOperatorControllerBindingLevel.accept(xbox.b(), Superstructure.State.L1);
501519

502520
xbox.povDown().onTrue(superstructure.stowLow());
503521

504-
coralIntake.setDefaultCommand(superstructure.passiveIntake());
522+
// Intake
505523

506-
// Hang
507-
hang.setDefaultCommand(hang.run(() -> hang.set(MathUtil.applyDeadband(xbox.getLeftX(), 0.2))));
524+
coralIntake.setDefaultCommand(superstructure.passiveIntake());
508525

509-
DoubleSupplier hangSpeed = () -> MathUtil.applyDeadband(xbox.getRightX(), 0.2);
510-
new Trigger(() -> hangSpeed.getAsDouble() != 0)
526+
DoubleSupplier intakeSpeed =
527+
() ->
528+
MathUtil.clamp(
529+
JoystickUtil.applyDeadband(-xbox.getLeftX())
530+
+ JoystickUtil.applyDeadband(-xbox.getLeftY()),
531+
-1,
532+
+1);
533+
new Trigger(() -> intakeSpeed.getAsDouble() != 0)
511534
.and(DriverStation::isTeleopEnabled)
512-
.whileTrue(Commands.run(() -> hang.set(hangSpeed.getAsDouble())));
535+
.whileTrue(coralIntake.run(() -> coralIntake.setMotors(intakeSpeed.getAsDouble())));
536+
537+
// Hang
538+
hang.setDefaultCommand(hang.run(() -> hang.set(JoystickUtil.applyDeadband(xbox.getLeftX()))));
513539

514540
xbox.rightBumper().and(xbox.leftBumper().negate()).onTrue(hang.deploy());
515541
xbox.leftBumper().and(xbox.rightBumper().negate()).onTrue(hang.retract());
516542
xbox.rightBumper().and(xbox.leftBumper()).onTrue(hang.stow());
517543
}
518544

519-
private void configureOperatorControllerBindingLevel(
520-
Trigger trigger, Superstructure.State state) {
521-
trigger.whileTrue(superstructure.run(state));
522-
}
523-
524545
private Command rumbleController(CommandXboxController controller, double rumbleIntensity) {
525546
return Commands.startEnd(
526547
() -> controller.setRumble(RumbleType.kBothRumble, rumbleIntensity),

‎src/main/java/frc/robot/commands/ManualAlignCommands.java‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
package frc.robot.commands;
2+
3+
import edu.wpi.first.math.MathUtil;
4+
import edu.wpi.first.math.geometry.Pose2d;
5+
import edu.wpi.first.math.geometry.Rotation2d;
6+
import edu.wpi.first.math.geometry.Translation2d;
7+
import edu.wpi.first.math.util.Units;
8+
import edu.wpi.first.wpilibj2.command.Command;
9+
import frc.robot.FieldConstants.Barge;
10+
import frc.robot.FieldConstants.CoralStation;
11+
import frc.robot.FieldConstants.Reef;
12+
import frc.robot.commands.controllers.JoystickInputController;
13+
import frc.robot.commands.controllers.SpeedLevelController.SpeedLevel;
14+
import frc.robot.subsystems.drive.Drive;
15+
import frc.robot.utility.AllianceFlipUtil;
16+
import java.util.List;
17+
import java.util.Optional;
18+
import java.util.stream.Stream;
19+
20+
public class ManualAlignCommands {
21+
22+
public static Command alignToCage(Drive drivetrain, JoystickInputController input) {
23+
return DriveCommands.joystickHeadingDrive(
24+
drivetrain,
25+
input::getTranslationMetersPerSecond,
26+
() -> Optional.of(allianceRotation().plus(Rotation2d.k180deg)),
27+
() -> SpeedLevel.NO_LEVEL,
28+
() -> true);
29+
}
30+
31+
public static Command alignToCageAdv(Drive drivetrain, JoystickInputController input) {
32+
return DriveCommands.joystickHeadingDrive(
33+
drivetrain,
34+
input::getTranslationMetersPerSecond,
35+
() -> {
36+
Translation2d robotPose = drivetrain.getRobotPose().getTranslation();
37+
38+
List<Translation2d> cagePose =
39+
Stream.of(Barge.farCage, Barge.middleCage, Barge.closeCage)
40+
.map(AllianceFlipUtil::apply)
41+
.toList();
42+
43+
return Optional.of(
44+
robotPose.minus(robotPose.nearest(cagePose)).getAngle().plus(allianceRotation()));
45+
},
46+
() -> SpeedLevel.NO_LEVEL,
47+
() -> true);
48+
}
49+
50+
public static Command alignToSourceLeft(Drive drivetrain, JoystickInputController input) {
51+
return DriveCommands.joystickHeadingDrive(
52+
drivetrain,
53+
input::getTranslationMetersPerSecond,
54+
() -> Optional.of(AllianceFlipUtil.apply(CoralStation.leftCenterFace.getRotation())),
55+
() -> SpeedLevel.NO_LEVEL,
56+
() -> true);
57+
}
58+
59+
public static Command alignToSourceRight(Drive drivetrain, JoystickInputController input) {
60+
return DriveCommands.joystickHeadingDrive(
61+
drivetrain,
62+
input::getTranslationMetersPerSecond,
63+
() -> Optional.of(AllianceFlipUtil.apply(CoralStation.rightCenterFace.getRotation())),
64+
() -> SpeedLevel.NO_LEVEL,
65+
() -> true);
66+
}
67+
68+
public static Command alignToReef(Drive drivetrain, JoystickInputController input) {
69+
return DriveCommands.joystickHeadingDrive(
70+
drivetrain,
71+
input::getTranslationMetersPerSecond,
72+
() -> {
73+
Translation2d robotPose = drivetrain.getRobotPose().getTranslation();
74+
Translation2d reefPose = AllianceFlipUtil.apply(Reef.center);
75+
76+
double angleRad = robotPose.minus(reefPose).getAngle().getRadians();
77+
78+
for (Pose2d face : Reef.alignmentFaces) {
79+
double faceRad = face.getRotation().getRadians();
80+
81+
if (MathUtil.isNear(faceRad, angleRad, Units.degreesToRadians(25))) {
82+
angleRad = faceRad;
83+
}
84+
}
85+
86+
return Optional.of(AllianceFlipUtil.apply(Rotation2d.fromRadians(angleRad)));
87+
},
88+
() -> SpeedLevel.NO_LEVEL,
89+
() -> true);
90+
}
91+
92+
private static Rotation2d allianceRotation() {
93+
return AllianceFlipUtil.shouldFlip() ? Rotation2d.kZero : Rotation2d.k180deg;
94+
}
95+
}

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

Lines changed: 27 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -78,17 +78,11 @@ public Superstructure(
7878
public Command run(State goal) {
7979
return runPrepare(goal)
8080
.andThen(Commands.idle(elevator, coralWrist))
81-
.finallyDo(
82-
() -> {
83-
elevator.setGoalHeightMeters(STOW_HEIGHT);
84-
coralWrist.setGoalRotation(STOW_CORAL_ANGLE);
85-
});
81+
.finallyDo(this::setPositionStow);
8682
}
8783

8884
public Command runAction(State newGoal) {
89-
return runPrepare(newGoal)
90-
.andThen(Commands.idle(elevator, coralWrist))
91-
.until(() -> elevator.atGoalHeight() && coralWrist.atGoal());
85+
return runPrepare(newGoal).andThen(Commands.idle(elevator, coralWrist)).until(this::atGoal);
9286
}
9387

9488
public Command runPrepare(State goal) {
@@ -103,8 +97,20 @@ public Command runPrepare(State goal) {
10397
};
10498
}
10599

100+
public Command runWheels(State goal) {
101+
return switch (goal) {
102+
case STOW -> stopIntake();
103+
case STOW_HIGH -> stopIntake();
104+
case L1 -> outtakeL1();
105+
case L2 -> outtake();
106+
case L3 -> outtake();
107+
case L4 -> outtake();
108+
case INTAKE -> intake();
109+
};
110+
}
111+
106112
public static final double L1_HEIGHT = 0.151148670108898;
107-
public static final Rotation2d L1_CORAL_ANGLE = Rotation2d.fromDegrees(70);
113+
public static final Rotation2d L1_CORAL_ANGLE = Rotation2d.fromDegrees(0);
108114

109115
public static final double L2_HEIGHT = 0.469491383230037 + Units.inchesToMeters(3);
110116
public static final Rotation2d L2_CORAL_ANGLE = Rotation2d.fromDegrees(-35);
@@ -120,6 +126,8 @@ public Command runPrepare(State goal) {
120126

121127
public static final double STOW_HEIGHT = 0.054886473109919;
122128
public static final Rotation2d STOW_CORAL_ANGLE = Rotation2d.fromDegrees(-90);
129+
130+
public static final double STOW_HEIGHT_HIGH = 0;
123131
public static final Rotation2d STOW_CORAL_ANGLE_HIGH = Rotation2d.fromDegrees(90);
124132

125133
// TODO: Integrate lightstrip commands into this stuff
@@ -156,7 +164,7 @@ public Command stowLow() {
156164

157165
public Command stowHigh() {
158166
return Commands.parallel(
159-
elevator.runPositionPrepare(STOW_HEIGHT),
167+
elevator.runPositionPrepare(STOW_HEIGHT_HIGH),
160168
coralWrist.runPositionPrepare(STOW_CORAL_ANGLE_HIGH));
161169
}
162170

@@ -180,6 +188,15 @@ public Command stopIntake() {
180188
return coralIntake.runMotors(0);
181189
}
182190

191+
public boolean atGoal() {
192+
return elevator.atGoalHeight() && coralWrist.atGoal();
193+
}
194+
195+
public void setPositionStow() {
196+
elevator.setGoalHeightMeters(STOW_HEIGHT_HIGH);
197+
coralWrist.setGoalRotation(STOW_CORAL_ANGLE_HIGH);
198+
}
199+
183200
@Override
184201
public void periodic() {
185202
SmartDashboard.putNumber("Elevator Height Meters", elevator.getMeasuredHeightMeters());

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

Lines changed: 15 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,11 +15,11 @@ public class SuperstructureVisualizer {
1515

1616
private final LoggedMechanism2d mechanism =
1717
new LoggedMechanism2d(
18-
Units.inchesToMeters(28.0), Units.feetToMeters(9.0), new Color8Bit(Color.kDarkGray));
18+
Units.inchesToMeters(28.0), Units.feetToMeters(5.0), new Color8Bit(Color.kDarkGray));
1919

2020
private final LoggedMechanismRoot2d root;
21-
private final LoggedMechanismLigament2d carriageMechanism;
2221
private final LoggedMechanismLigament2d elevatorMechanism;
22+
private final LoggedMechanismLigament2d carriage, carriageJoint;
2323
private final LoggedMechanismLigament2d coralWristLigament;
2424
private final LoggedMechanismLigament2d coralIntake;
2525

@@ -33,7 +33,7 @@ public SuperstructureVisualizer(String name, Color color) {
3333
new LoggedMechanismLigament2d(
3434
name + " Elevator", Units.inchesToMeters(26.0), 90, 4.0, new Color8Bit(color)));
3535

36-
carriageMechanism =
36+
carriage =
3737
elevatorMechanism.append(
3838
new LoggedMechanismLigament2d(
3939
name + " Carriage",
@@ -42,10 +42,19 @@ public SuperstructureVisualizer(String name, Color color) {
4242
4.0,
4343
new Color8Bit(color)));
4444

45+
carriageJoint =
46+
carriage.append(
47+
new LoggedMechanismLigament2d(
48+
name + " Carriage Joint",
49+
Units.inchesToMeters(4),
50+
90.0,
51+
4.0,
52+
new Color8Bit(color)));
53+
4554
coralWristLigament =
46-
carriageMechanism.append(
55+
carriageJoint.append(
4756
new LoggedMechanismLigament2d(
48-
name + " Algae Wrist", Units.inchesToMeters(9.0), 0.0, 4.0, new Color8Bit(color)));
57+
name + " Algae Wrist", Units.inchesToMeters(9.0), 0, 4.0, new Color8Bit(color)));
4958

5059
coralIntake =
5160
coralWristLigament.append(
@@ -55,7 +64,7 @@ public SuperstructureVisualizer(String name, Color color) {
5564

5665
public void update(double carriageHeight, double wristRadians, double coralIntakeOutput) {
5766
elevatorMechanism.setLength(carriageHeight);
58-
coralWristLigament.setAngle(new Rotation2d(-wristRadians).plus(Rotation2d.kCCW_Pi_2));
67+
coralWristLigament.setAngle(new Rotation2d(-wristRadians));
5968

6069
coralIntake.setLength(coralIntakeOutput * Units.inchesToMeters(3.0));
6170

‎src/main/java/frc/robot/subsystems/superstructure/algae/Algae.java‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
package frc.robot.subsystems.superstructure.algae;
2+
3+
import edu.wpi.first.wpilibj.Alert;
4+
import edu.wpi.first.wpilibj2.command.Command;
5+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
6+
import frc.robot.subsystems.superstructure.intake.IntakeIO;
7+
import frc.robot.subsystems.superstructure.intake.IntakeIOInputsAutoLogged;
8+
import org.littletonrobotics.junction.Logger;
9+
10+
public class Algae extends SubsystemBase {
11+
12+
private final IntakeIO io;
13+
private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
14+
15+
private final Alert motorDisconnectedAlert;
16+
17+
public Algae(IntakeIO io) {
18+
this.io = io;
19+
20+
motorDisconnectedAlert = new Alert("Algae motor/s disconnected!", Alert.AlertType.kWarning);
21+
}
22+
23+
@Override
24+
public void periodic() {
25+
io.updateInputs(inputs);
26+
Logger.processInputs("Intake", inputs);
27+
28+
motorDisconnectedAlert.set(!inputs.motorConnected);
29+
}
30+
31+
public Command runMotors() {
32+
return runEnd(
33+
() -> {
34+
io.setLeftMotor(AlgaeConstants.SPEED);
35+
io.setRightMotor(AlgaeConstants.SPEED);
36+
},
37+
io::stopMotors);
38+
}
39+
}

0 commit comments

Comments
 (0)