Skip to content
This repository was archived by the owner on Jan 17, 2026. It is now read-only.

Commit a929694

Browse files
SaxomophoneSaxomophone
committed
Merge branch 'master' of https://github.com/CurtinFRC/2025-Reefscape into ava/contextful-xbox-controller
2 parents 7b094aa + 9c87780 commit a929694

File tree

4 files changed

+107
-12
lines changed

4 files changed

+107
-12
lines changed

‎src/main/java/org/curtinfrc/frc2025/Robot.java‎

Lines changed: 56 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,11 @@
1010
import edu.wpi.first.net.WebServer;
1111
import edu.wpi.first.wpilibj.Alert;
1212
import edu.wpi.first.wpilibj.Alert.AlertType;
13+
import edu.wpi.first.wpilibj.DataLogManager;
14+
import edu.wpi.first.wpilibj.DriverStation;
1315
import edu.wpi.first.wpilibj.Filesystem;
1416
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
17+
import edu.wpi.first.wpilibj.RobotBase;
1518
import edu.wpi.first.wpilibj.Threads;
1619
import edu.wpi.first.wpilibj2.command.Command;
1720
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -111,6 +114,7 @@ public class Robot extends LoggedRobot {
111114

112115
private final List<Pose2d> leftSetpoints;
113116
private final List<Pose2d> rightSetpoints;
117+
private final List<Pose2d> l1Setpoints;
114118
private final List<Pose2d> algaeSetpoints;
115119

116120
public Robot() {
@@ -157,10 +161,15 @@ public Robot() {
157161

158162
SignalLogger.start();
159163
SignalLogger.setPath("/U/logs");
164+
DataLogManager.start();
160165
Logger.registerURCL(URCL.startExternal());
161166
// Start AdvantageKit logger
162167
Logger.start();
163168

169+
if (!RobotBase.isSimulation()) {
170+
DriverStation.waitForDsConnection(300);
171+
}
172+
164173
if (Constants.getMode() != Mode.REPLAY) {
165174
switch (Constants.robotType) {
166175
case COMPBOT -> {
@@ -265,6 +274,21 @@ public Robot() {
265274
rightSetpoints =
266275
List.of(B.getPose(), D.getPose(), F.getPose(), H.getPose(), J.getPose(), L.getPose());
267276

277+
l1Setpoints =
278+
List.of(
279+
A_LEFTL1.getPose(),
280+
A_RIGHTL1.getPose(),
281+
C_LEFTL1.getPose(),
282+
C_RIGHTL1.getPose(),
283+
E_LEFTL1.getPose(),
284+
E_RIGHTL1.getPose(),
285+
G_LEFTL1.getPose(),
286+
G_RIGHTL1.getPose(),
287+
I_LEFTL1.getPose(),
288+
I_RIGHTL1.getPose(),
289+
K_LEFTL1.getPose(),
290+
K_RIGHTL1.getPose());
291+
268292
algaeSetpoints =
269293
List.of(
270294
CLOSE.getPose(),
@@ -374,9 +398,9 @@ public Robot() {
374398
drive
375399
.autoAlignWithOverride(
376400
() -> DriveSetpoints.closest(drive::getPose, rightSetpoints),
377-
() -> -controller.getLeftY(),
378-
() -> -controller.getLeftX(),
379-
() -> -controller.getRightX())
401+
() -> controller.getLeftY(),
402+
() -> controller.getLeftX(),
403+
() -> controller.getRightX())
380404
.until(ejector.backSensor.negate()));
381405

382406
manualController.leftBumper().or(controller.leftTrigger()).whileTrue(ejector.eject(30));
@@ -402,9 +426,9 @@ public Robot() {
402426
drive
403427
.autoAlignWithOverride(
404428
() -> DriveSetpoints.closest(drive::getPose, leftSetpoints),
405-
() -> -controller.getLeftY(),
406-
() -> -controller.getLeftX(),
407-
() -> -controller.getRightX())
429+
() -> controller.getLeftY(),
430+
() -> controller.getLeftX(),
431+
() -> controller.getRightX())
408432
.until(ejector.backSensor.negate()));
409433

410434
climber.stalled.onTrue(
@@ -441,14 +465,14 @@ public Robot() {
441465
.andThen(Commands.runOnce(() -> controller.setRumble(RumbleType.kBothRumble, 0.0))));
442466

443467
controller
444-
.leftStick()
468+
.povLeft()
445469
.whileTrue(
446470
Commands.parallel(
447471
drive.autoAlignWithOverride(
448472
() -> DriveSetpoints.closest(drive::getPose, algaeSetpoints),
449-
() -> -controller.getLeftY(),
450-
() -> -controller.getLeftX(),
451-
() -> -controller.getRightX()),
473+
() -> controller.getLeftY(),
474+
() -> controller.getLeftX(),
475+
() -> controller.getRightX()),
452476
ejector.eject(40),
453477
elevator.goToSetpoint(
454478
() -> {
@@ -465,6 +489,28 @@ public Robot() {
465489
intake.backSensor.negate()))
466490
.withName("AlgaePop")
467491
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
492+
493+
controller
494+
.povRight()
495+
.and(override.negate())
496+
.whileTrue(
497+
Commands.sequence(
498+
Commands.parallel(
499+
drive.autoAlignWithOverride(
500+
() -> DriveSetpoints.closest(drive::getPose, l1Setpoints),
501+
() -> controller.getLeftY(),
502+
() -> controller.getLeftX(),
503+
() -> controller.getRightX()),
504+
elevator.goToSetpoint(ElevatorSetpoints.L1, intake.backSensor.negate()))
505+
.until(elevator.atSetpoint.and(drive.atSetpoint)),
506+
Commands.parallel(
507+
drive.autoAlignWithOverride(
508+
() -> DriveSetpoints.closest(drive::getPose, l1Setpoints),
509+
() -> controller.getLeftY(),
510+
() -> controller.getLeftX(),
511+
() -> controller.getRightX()),
512+
elevator.goToSetpoint(ElevatorSetpoints.L1, intake.backSensor.negate()),
513+
ejector.eject(25))));
468514
controller
469515
.rightStickRaw()
470516
.whileTrue(ejector.eject(15).withInterruptBehavior(InterruptionBehavior.kCancelIncoming));

‎src/main/java/org/curtinfrc/frc2025/subsystems/drive/DriveConstants.java‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020

2121
public final class DriveConstants {
2222
public static final double coralOffset = 0.32 / 2;
23+
public static final double l1Offset = 45.5;
2324
public static final double algaeOffset = 0.155;
2425
public static final double DEADBAND = 0;
2526
public static final double ANGLE_KP = 5.0;
@@ -48,16 +49,28 @@ public final class DriveConstants {
4849

4950
public static enum DriveSetpoints implements StructSerializable {
5051
A(aprilTagLayout.getTagPose(18).get(), true, coralOffset),
52+
A_LEFTL1(aprilTagLayout.getTagPose(18).get(), true, l1Offset),
53+
A_RIGHTL1(aprilTagLayout.getTagPose(18).get(), false, l1Offset),
5154
B(aprilTagLayout.getTagPose(18).get(), false, coralOffset),
5255
C(aprilTagLayout.getTagPose(17).get(), true, coralOffset),
56+
C_LEFTL1(aprilTagLayout.getTagPose(17).get(), true, l1Offset),
57+
C_RIGHTL1(aprilTagLayout.getTagPose(17).get(), false, l1Offset),
5358
D(aprilTagLayout.getTagPose(17).get(), false, coralOffset),
5459
E(aprilTagLayout.getTagPose(22).get(), true, coralOffset),
60+
E_LEFTL1(aprilTagLayout.getTagPose(22).get(), true, l1Offset),
61+
E_RIGHTL1(aprilTagLayout.getTagPose(22).get(), false, l1Offset),
5562
F(aprilTagLayout.getTagPose(22).get(), false, coralOffset),
5663
G(aprilTagLayout.getTagPose(21).get(), true, coralOffset),
64+
G_LEFTL1(aprilTagLayout.getTagPose(21).get(), true, l1Offset),
65+
G_RIGHTL1(aprilTagLayout.getTagPose(21).get(), false, l1Offset),
5766
H(aprilTagLayout.getTagPose(21).get(), false, coralOffset),
5867
I(aprilTagLayout.getTagPose(20).get(), true, coralOffset),
68+
I_LEFTL1(aprilTagLayout.getTagPose(20).get(), true, l1Offset),
69+
I_RIGHTL1(aprilTagLayout.getTagPose(20).get(), false, l1Offset),
5970
J(aprilTagLayout.getTagPose(20).get(), false, coralOffset),
6071
K(aprilTagLayout.getTagPose(19).get(), true, coralOffset),
72+
K_LEFTL1(aprilTagLayout.getTagPose(19).get(), true, l1Offset),
73+
K_RIGHTL1(aprilTagLayout.getTagPose(19).get(), false, l1Offset),
6174
L(aprilTagLayout.getTagPose(19).get(), false, coralOffset),
6275
CLOSE(aprilTagLayout.getTagPose(18).get(), true, algaeOffset),
6376
CLOSE_LEFT(aprilTagLayout.getTagPose(19).get(), true, algaeOffset),
@@ -193,6 +206,42 @@ public static DriveSetpoints closest(Supplier<Pose2d> currentPose, List<Pose2d>
193206
if (desiredPose.equals(CLOSE_LEFT.getPose())) {
194207
return CLOSE_LEFT;
195208
}
209+
if (desiredPose.equals(A_LEFTL1.getPose())) {
210+
return A_LEFTL1;
211+
}
212+
if (desiredPose.equals(A_RIGHTL1.getPose())) {
213+
return A_RIGHTL1;
214+
}
215+
if (desiredPose.equals(C_LEFTL1.getPose())) {
216+
return C_LEFTL1;
217+
}
218+
if (desiredPose.equals(C_RIGHTL1.getPose())) {
219+
return C_RIGHTL1;
220+
}
221+
if (desiredPose.equals(E_LEFTL1.getPose())) {
222+
return E_LEFTL1;
223+
}
224+
if (desiredPose.equals(E_RIGHTL1.getPose())) {
225+
return E_RIGHTL1;
226+
}
227+
if (desiredPose.equals(G_LEFTL1.getPose())) {
228+
return G_LEFTL1;
229+
}
230+
if (desiredPose.equals(G_RIGHTL1.getPose())) {
231+
return G_RIGHTL1;
232+
}
233+
if (desiredPose.equals(I_LEFTL1.getPose())) {
234+
return I_LEFTL1;
235+
}
236+
if (desiredPose.equals(I_RIGHTL1.getPose())) {
237+
return I_RIGHTL1;
238+
}
239+
if (desiredPose.equals(K_LEFTL1.getPose())) {
240+
return K_LEFTL1;
241+
}
242+
if (desiredPose.equals(K_RIGHTL1.getPose())) {
243+
return K_RIGHTL1;
244+
}
196245
throw new IllegalStateException("No matching setpoint found!");
197246
}
198247
}

‎src/main/java/org/curtinfrc/frc2025/subsystems/ejector/EjectorIOComp.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ public class EjectorIOComp implements EjectorIO {
2424
private static final int ID = 53;
2525
private static final int FOLLOWER_ID = 46;
2626
private static final CurrentLimitsConfigs currentLimits =
27-
new CurrentLimitsConfigs().withSupplyCurrentLimit(20).withStatorCurrentLimit(40);
27+
new CurrentLimitsConfigs().withSupplyCurrentLimit(30).withStatorCurrentLimit(60);
2828
private static final TalonFXConfiguration leaderConfig =
2929
new TalonFXConfiguration()
3030
.withMotorOutput(

‎src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ public class ElevatorConstants {
1818
public static double climbkD = 0;
1919

2020
public static enum ElevatorSetpoints implements StructSerializable {
21-
L1(0),
21+
L1(0.03),
2222
L2(0.221),
2323
AlgaePopLow(0),
2424
L3(0.611),

0 commit comments

Comments
 (0)