Skip to content

Commit cf24a64

Browse files
AceiusRedshiftAceiusRedshift
committed
Merge branch 'main' into keep-at-range
2 parents ed4e9b5 + 1061486 commit cf24a64

File tree

14 files changed

+294
-36
lines changed

14 files changed

+294
-36
lines changed

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

Lines changed: 32 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@
44

55
import com.pathplanner.lib.auto.AutoBuilder;
66
import com.pathplanner.lib.auto.NamedCommands;
7-
import edu.wpi.first.math.MathUtil;
87
import edu.wpi.first.math.geometry.Pose2d;
98
import edu.wpi.first.math.geometry.Rotation2d;
109
import edu.wpi.first.math.geometry.Transform2d;
@@ -25,6 +24,7 @@
2524
import frc.robot.Constants.Mode;
2625
import frc.robot.commands.AdaptiveAutoAlignCommands;
2726
import frc.robot.commands.DriveCommands;
27+
import frc.robot.commands.ManualAlignCommands;
2828
import frc.robot.commands.controllers.JoystickInputController;
2929
import frc.robot.commands.controllers.SpeedLevelController;
3030
import frc.robot.subsystems.addressableled.AddressableLEDSubsystem;
@@ -64,10 +64,11 @@
6464
import frc.robot.subsystems.vision.CameraIOPhotonVision;
6565
import frc.robot.subsystems.vision.CameraIOSim;
6666
import frc.robot.subsystems.vision.VisionConstants;
67+
import frc.robot.utility.JoystickUtil;
6768
import frc.robot.utility.OverrideSwitch;
6869
import frc.robot.utility.commands.CustomCommands;
6970
import java.util.Arrays;
70-
import java.util.function.DoubleSupplier;
71+
import java.util.function.BiConsumer;
7172
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
7273

7374
/**
@@ -413,6 +414,18 @@ private void configureDriverControllerBindings(
413414
.ignoringDisable(true)
414415
.withName("Reset Gyro Heading"));
415416

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

@@ -480,7 +493,7 @@ private void configureOperatorControllerBindings(CommandXboxController xbox) {
480493

481494
new Trigger(DriverStation::isEnabled)
482495
.onTrue(
483-
Commands.runOnce(() -> coralWrist.setConstraints(3, 8))
496+
Commands.runOnce(() -> coralWrist.setConstraints(1, 3))
484497
.andThen(superstructure.runAction(Superstructure.State.STOW))
485498
.finallyDo(coralWrist::resetContraints));
486499

@@ -493,34 +506,35 @@ private void configureOperatorControllerBindings(CommandXboxController xbox) {
493506
xbox.rightTrigger().and(xbox.a().negate()).whileTrue(superstructure.outtake());
494507
xbox.rightTrigger().and(xbox.a()).whileTrue(superstructure.outtakeL1());
495508

496-
configureOperatorControllerBindingLevel(xbox.y(), Superstructure.State.L4);
497-
configureOperatorControllerBindingLevel(xbox.x(), Superstructure.State.L3);
498-
configureOperatorControllerBindingLevel(xbox.a(), Superstructure.State.L2);
509+
final BiConsumer<Trigger, Superstructure.State> configureOperatorControllerBindingLevel =
510+
(trigger, level) -> {
511+
trigger.whileTrue(superstructure.run(level));
512+
};
499513

500-
configureOperatorControllerBindingLevel(xbox.b(), Superstructure.State.L1);
514+
configureOperatorControllerBindingLevel.accept(xbox.y(), Superstructure.State.L4);
515+
configureOperatorControllerBindingLevel.accept(xbox.x(), Superstructure.State.L3);
516+
configureOperatorControllerBindingLevel.accept(xbox.a(), Superstructure.State.L2);
517+
configureOperatorControllerBindingLevel.accept(xbox.b(), Superstructure.State.L1);
501518

502519
xbox.povDown().onTrue(superstructure.stowLow());
503520

521+
// Intake
522+
504523
coralIntake.setDefaultCommand(superstructure.passiveIntake());
524+
new Trigger(() -> !JoystickUtil.isDeadband(xbox.getLeftX()))
525+
.and(DriverStation::isTeleopEnabled)
526+
.whileTrue(
527+
Commands.run(
528+
() -> coralIntake.setMotors(JoystickUtil.applyDeadband(xbox.getRightX()))));
505529

506530
// Hang
507-
hang.setDefaultCommand(hang.run(() -> hang.set(MathUtil.applyDeadband(xbox.getLeftX(), 0.2))));
508-
509-
DoubleSupplier hangSpeed = () -> MathUtil.applyDeadband(xbox.getRightX(), 0.2);
510-
new Trigger(() -> hangSpeed.getAsDouble() != 0)
511-
.and(DriverStation::isTeleopEnabled)
512-
.whileTrue(Commands.run(() -> hang.set(hangSpeed.getAsDouble())));
531+
hang.setDefaultCommand(hang.run(() -> hang.set(JoystickUtil.applyDeadband(xbox.getLeftX()))));
513532

514533
xbox.rightBumper().and(xbox.leftBumper().negate()).onTrue(hang.deploy());
515534
xbox.leftBumper().and(xbox.rightBumper().negate()).onTrue(hang.retract());
516535
xbox.rightBumper().and(xbox.leftBumper()).onTrue(hang.stow());
517536
}
518537

519-
private void configureOperatorControllerBindingLevel(
520-
Trigger trigger, Superstructure.State state) {
521-
trigger.whileTrue(superstructure.run(state));
522-
}
523-
524538
private Command rumbleController(CommandXboxController controller, double rumbleIntensity) {
525539
return Commands.startEnd(
526540
() -> 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: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ public Command runPrepare(State goal) {
104104
}
105105

106106
public static final double L1_HEIGHT = 0.151148670108898;
107-
public static final Rotation2d L1_CORAL_ANGLE = Rotation2d.fromDegrees(70);
107+
public static final Rotation2d L1_CORAL_ANGLE = Rotation2d.fromDegrees(0);
108108

109109
public static final double L2_HEIGHT = 0.469491383230037 + Units.inchesToMeters(3);
110110
public static final Rotation2d L2_CORAL_ANGLE = Rotation2d.fromDegrees(-35);

‎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+
}

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

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
package frc.robot.subsystems.superstructure.algae;
2+
3+
import frc.robot.Constants;
4+
5+
/** Constants for the Template subsystem. */
6+
public class AlgaeConstants {
7+
8+
public static double SPEED = 0.5;
9+
10+
public record IntakeConfig(
11+
int motorIdLeft, int motorIdRight, boolean invertedLeft, boolean invertedRight) {}
12+
;
13+
14+
public static final IntakeConfig CORAL_INTAKE_CONFIG =
15+
switch (Constants.getRobot()) {
16+
case COMP_BOT_2025 -> new IntakeConfig(4, 7, false, true);
17+
default -> new IntakeConfig(0, 0, false, false);
18+
};
19+
20+
public static final IntakeConfig ALGAE_INTAKE_CONFIG =
21+
switch (Constants.getRobot()) {
22+
case COMP_BOT_2025 -> new IntakeConfig(5, 6, false, false);
23+
default -> new IntakeConfig(0, 0, false, false);
24+
};
25+
}

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ public static record ElevatorConfig(int leaderCanId, int followerCanId, boolean
1616
default -> new ElevatorConfig(0, 1, false);
1717
};
1818

19-
public static final int currentLimit = 35;
19+
public static final int currentLimit = 40;
2020
public static final double gearReduction = 9.0;
2121

2222
public static final double elevatorHeight = 1.245 + Units.inchesToMeters(8.5);
@@ -31,7 +31,7 @@ public static record ElevatorConfig(int leaderCanId, int followerCanId, boolean
3131
public static final double maxCarriageVelocity = 3.0;
3232
public static final double maxCarriageAcceleration = 3.5;
3333

34-
public static final double carriagePositionTolerance = Units.inchesToMeters(2);
34+
public static final double carriagePositionTolerance = Units.inchesToMeters(1);
3535

3636
public static final PIDConstants pid =
3737
switch (Constants.getRobot()) {

‎src/main/java/frc/robot/subsystems/superstructure/intake/sensor/Sensor.java‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
package frc.robot.subsystems.superstructure.intake.sensor;
2+
3+
import edu.wpi.first.math.filter.Debouncer;
4+
import edu.wpi.first.wpilibj.Alert;
5+
6+
public class Sensor {
7+
8+
private final SensorIO io;
9+
private final SensorIO.SensorIOInputs inputs = new SensorIO.SensorIOInputs();
10+
11+
private final Alert disconnectedAlert =
12+
new Alert("Intake sensor disconnected!", Alert.AlertType.kWarning);
13+
14+
private final Debouncer debouncer = new Debouncer(0.1);
15+
private boolean hasItem = false;
16+
17+
public Sensor(SensorIO io) {
18+
this.io = io;
19+
}
20+
21+
public void periodic() {
22+
io.updateInputs(inputs);
23+
24+
hasItem = debouncer.calculate(inputs.detected);
25+
26+
disconnectedAlert.set(!inputs.connected);
27+
}
28+
29+
public boolean isDetected() {
30+
return hasItem;
31+
}
32+
33+
/** For simulation, request item */
34+
public void simulateItemRequest() {
35+
io.simulateItemDesire();
36+
}
37+
38+
/** For simulation, eject item */
39+
public void simulateItemEjection() {
40+
io.simulateItemEjection();
41+
}
42+
}

‎src/main/java/frc/robot/subsystems/superstructure/intake/sensor/SensorIO.java‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,13 @@ public static class SensorIOInputs {
88
boolean connected = false;
99

1010
double rawValue = 0.0;
11-
boolean rawDetected = false;
12-
13-
boolean hasItem;
11+
boolean detected = false;
1412
}
1513

1614
/** Updates the set of loggable inputs. */
1715
default void updateInputs(SensorIOInputs inputs) {}
16+
17+
default void simulateItemDesire() {}
18+
19+
default void simulateItemEjection() {}
1820
}

‎src/main/java/frc/robot/subsystems/superstructure/intake/sensor/SensorIOBeam.java‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
package frc.robot.subsystems.superstructure.intake.sensor;
22

33
import edu.wpi.first.math.MathUtil;
4-
import edu.wpi.first.math.filter.Debouncer;
54
import edu.wpi.first.wpilibj.AnalogInput;
65

76
public class SensorIOBeam implements SensorIO {
@@ -10,16 +9,13 @@ public class SensorIOBeam implements SensorIO {
109
public static final double SENSOR_VOLTAGE_TOLERANCE = 0.4;
1110

1211
private final AnalogInput input = new AnalogInput(0);
13-
private final Debouncer debouncer = new Debouncer(0.1);
1412

1513
@Override
1614
public void updateInputs(SensorIOInputs inputs) {
1715
inputs.connected = true;
1816

1917
inputs.rawValue = input.getValue();
20-
inputs.rawDetected =
18+
inputs.detected =
2119
MathUtil.isNear(inputs.rawValue, SIGNAL_SENSOR_OCCUPIED, SENSOR_VOLTAGE_TOLERANCE);
22-
23-
inputs.hasItem = debouncer.calculate(inputs.rawDetected);
2420
}
2521
}

0 commit comments

Comments
 (0)