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

Commit 9c87780

Browse files
spacey-sootyspacey-sooty
authored
Fixes from driver practice (#143)
Signed-off-by: Jade Turner <[email protected]>
1 parent dd4e6fc commit 9c87780

File tree

4 files changed

+108
-13
lines changed

4 files changed

+108
-13
lines changed

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

Lines changed: 57 additions & 11 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;
@@ -102,6 +105,7 @@ public class Robot extends LoggedRobot {
102105

103106
private final List<Pose2d> leftSetpoints;
104107
private final List<Pose2d> rightSetpoints;
108+
private final List<Pose2d> l1Setpoints;
105109
private final List<Pose2d> algaeSetpoints;
106110

107111
@AutoLogOutput(key = "Robot/Overridden")
@@ -154,10 +158,15 @@ public Robot() {
154158

155159
SignalLogger.start();
156160
SignalLogger.setPath("/U/logs");
161+
DataLogManager.start();
157162
Logger.registerURCL(URCL.startExternal());
158163
// Start AdvantageKit logger
159164
Logger.start();
160165

166+
if (!RobotBase.isSimulation()) {
167+
DriverStation.waitForDsConnection(300);
168+
}
169+
161170
if (Constants.getMode() != Mode.REPLAY) {
162171
switch (Constants.robotType) {
163172
case COMPBOT -> {
@@ -262,6 +271,21 @@ public Robot() {
262271
rightSetpoints =
263272
List.of(B.getPose(), D.getPose(), F.getPose(), H.getPose(), J.getPose(), L.getPose());
264273

274+
l1Setpoints =
275+
List.of(
276+
A_LEFTL1.getPose(),
277+
A_RIGHTL1.getPose(),
278+
C_LEFTL1.getPose(),
279+
C_RIGHTL1.getPose(),
280+
E_LEFTL1.getPose(),
281+
E_RIGHTL1.getPose(),
282+
G_LEFTL1.getPose(),
283+
G_RIGHTL1.getPose(),
284+
I_LEFTL1.getPose(),
285+
I_RIGHTL1.getPose(),
286+
K_LEFTL1.getPose(),
287+
K_RIGHTL1.getPose());
288+
265289
algaeSetpoints =
266290
List.of(
267291
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
controller.leftBumper().or(controller.leftTrigger()).and(override).whileTrue(ejector.eject(30));
@@ -407,9 +431,9 @@ public Robot() {
407431
drive
408432
.autoAlignWithOverride(
409433
() -> DriveSetpoints.closest(drive::getPose, leftSetpoints),
410-
() -> -controller.getLeftY(),
411-
() -> -controller.getLeftX(),
412-
() -> -controller.getRightX())
434+
() -> controller.getLeftY(),
435+
() -> controller.getLeftX(),
436+
() -> controller.getRightX())
413437
.until(ejector.backSensor.negate()));
414438

415439
climber.stalled.onTrue(
@@ -446,15 +470,15 @@ public Robot() {
446470
.andThen(Commands.runOnce(() -> controller.setRumble(RumbleType.kBothRumble, 0.0))));
447471

448472
controller
449-
.leftStick()
473+
.povLeft()
450474
.and(override.negate())
451475
.whileTrue(
452476
Commands.parallel(
453477
drive.autoAlignWithOverride(
454478
() -> DriveSetpoints.closest(drive::getPose, algaeSetpoints),
455-
() -> -controller.getLeftY(),
456-
() -> -controller.getLeftX(),
457-
() -> -controller.getRightX()),
479+
() -> controller.getLeftY(),
480+
() -> controller.getLeftX(),
481+
() -> controller.getRightX()),
458482
ejector.eject(40),
459483
elevator.goToSetpoint(
460484
() -> {
@@ -471,8 +495,30 @@ public Robot() {
471495
intake.backSensor.negate()))
472496
.withName("AlgaePop")
473497
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
498+
474499
controller
475-
.rightStick()
500+
.povRight()
501+
.and(override.negate())
502+
.whileTrue(
503+
Commands.sequence(
504+
Commands.parallel(
505+
drive.autoAlignWithOverride(
506+
() -> DriveSetpoints.closest(drive::getPose, l1Setpoints),
507+
() -> controller.getLeftY(),
508+
() -> controller.getLeftX(),
509+
() -> controller.getRightX()),
510+
elevator.goToSetpoint(ElevatorSetpoints.L1, intake.backSensor.negate()))
511+
.until(elevator.atSetpoint.and(drive.atSetpoint)),
512+
Commands.parallel(
513+
drive.autoAlignWithOverride(
514+
() -> DriveSetpoints.closest(drive::getPose, l1Setpoints),
515+
() -> controller.getLeftY(),
516+
() -> controller.getLeftX(),
517+
() -> controller.getRightX()),
518+
elevator.goToSetpoint(ElevatorSetpoints.L1, intake.backSensor.negate()),
519+
ejector.eject(25))));
520+
controller
521+
.leftStick()
476522
.whileTrue(ejector.eject(15).withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
477523
controller.povUp().whileTrue(intake.intake(-4));
478524

‎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)