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

Commit 6b1ab10

Browse files
spacey-sootyspacey-sooty
committed
Move elevator safety sensor to elevator
Signed-off-by: Jade Turner <[email protected]>
1 parent fe5e1a4 commit 6b1ab10

File tree

11 files changed

+77
-166
lines changed

11 files changed

+77
-166
lines changed

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

Lines changed: 29 additions & 102 deletions
Original file line numberDiff line numberDiff line change
@@ -41,18 +41,17 @@ public static AutoRoutine onePieceCentre(
4141
waitSeconds(1),
4242
parallel(
4343
drive.autoAlign(() -> DriveSetpoints.G.getPose()),
44-
elevator.goToSetpoint(ElevatorSetpoints.L3, () -> true))
44+
elevator.goToSetpoint(ElevatorSetpoints.L3))
4545
.until(elevator.atSetpoint.and(drive.atSetpoint)),
4646
parallel(
4747
drive.autoAlign(() -> DriveSetpoints.G.getPose()),
48-
elevator.goToSetpoint(ElevatorSetpoints.L3, () -> true),
48+
elevator.goToSetpoint(ElevatorSetpoints.L3),
4949
ejector.eject(30))
5050
.until(ejector.backSensor.negate()),
5151
parallel(
5252
drive.autoAlign(() -> DriveSetpoints.FAR.getPose()),
5353
ejector.eject(40),
54-
elevator.goToSetpoint(
55-
() -> ElevatorSetpoints.AlgaePopLow, intake.backSensor.negate()))
54+
elevator.goToSetpoint(() -> ElevatorSetpoints.AlgaePopLow))
5655
.withTimeout(2)
5756
.withName("AlgaePop"),
5857
drive
@@ -76,7 +75,7 @@ public static AutoRoutine onePieceCentre(
7675
// .until(drive.atSetpoint)
7776
// .andThen(
7877
// elevator
79-
// .goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
78+
// .goToSetpoint(ElevatorSetpoints.L3)
8079
// .until(elevator.atSetpoint)
8180
// .andThen(
8281
// parallel(ejector.eject(15)),
@@ -85,7 +84,7 @@ public static AutoRoutine onePieceCentre(
8584
// .until(ejector.backSensor.negate()))
8685
// .andThen(
8786
// elevator
88-
// .goToSetpoint(ElevatorSetpoints.BASE, intake.backSensor.negate())
87+
// .goToSetpoint(ElevatorSetpoints.BASE)
8988
// .until(elevator.atSetpoint))
9089
// .andThen(
9190
// parallel(drive.autoAlign(() -> DriveSetpoints.FAR.getPose()),
@@ -111,10 +110,7 @@ public static AutoRoutine onePieceLeft(
111110
.until(ejector.backSensor.negate()));
112111
trajectory
113112
.atTime("RaiseElevator")
114-
.onTrue(
115-
elevator
116-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
117-
.until(ejector.backSensor.negate()));
113+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
118114
return routine;
119115
}
120116

@@ -140,10 +136,7 @@ public static AutoRoutine twoPieceLeft(
140136

141137
startToFirst
142138
.atTime("RaiseElevator")
143-
.onTrue(
144-
elevator
145-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
146-
.until(ejector.backSensor.negate()));
139+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
147140

148141
firstToHP
149142
.done()
@@ -166,10 +159,7 @@ public static AutoRoutine twoPieceLeft(
166159

167160
hpToSecond
168161
.atTime("RaiseElevator")
169-
.onTrue(
170-
elevator
171-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
172-
.until(ejector.backSensor.negate()));
162+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
173163

174164
return routine;
175165
}
@@ -199,10 +189,7 @@ public static AutoRoutine threePieceLeft(
199189

200190
startToFirst
201191
.atTime("RaiseElevator")
202-
.onTrue(
203-
elevator
204-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
205-
.until(ejector.backSensor.negate()));
192+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
206193

207194
firstToHP
208195
.done()
@@ -235,10 +222,7 @@ public static AutoRoutine threePieceLeft(
235222

236223
hpToSecond
237224
.atTime("RaiseElevator")
238-
.onTrue(
239-
elevator
240-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
241-
.until(ejector.backSensor.negate()));
225+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
242226

243227
hpToThird
244228
.done()
@@ -250,21 +234,15 @@ public static AutoRoutine threePieceLeft(
250234
ejector.eject(20).until(ejector.backSensor.negate()),
251235
parallel(
252236
drive.autoAlign(DriveSetpoints.CLOSE_LEFT::getPose),
253-
elevator
254-
.goToSetpoint(
255-
ElevatorSetpoints.AlgaePopLow, intake.backSensor.negate())
256-
.asProxy(),
237+
elevator.goToSetpoint(ElevatorSetpoints.AlgaePopLow).asProxy(),
257238
ejector.eject(30))
258239
.withTimeout(1),
259240
kToHp.cmd())
260241
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
261242

262243
hpToThird
263244
.atTime("RaiseElevator")
264-
.onTrue(
265-
elevator
266-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
267-
.until(ejector.backSensor.negate()));
245+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
268246

269247
return routine;
270248
}
@@ -296,10 +274,7 @@ public static AutoRoutine fourPieceLeft(
296274

297275
startToI
298276
.atTime("RaiseElevator")
299-
.onTrue(
300-
elevator
301-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
302-
.until(ejector.backSensor.negate()));
277+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
303278

304279
iToHp
305280
.done()
@@ -332,10 +307,7 @@ public static AutoRoutine fourPieceLeft(
332307

333308
hpToJ
334309
.atTime("RaiseElevator")
335-
.onTrue(
336-
elevator
337-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
338-
.until(ejector.backSensor.negate()));
310+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
339311

340312
hpToL
341313
.done()
@@ -359,10 +331,7 @@ public static AutoRoutine fourPieceLeft(
359331

360332
hpToL
361333
.atTime("RaiseElevator")
362-
.onTrue(
363-
elevator
364-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
365-
.until(ejector.backSensor.negate()));
334+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
366335

367336
hpToK
368337
.done()
@@ -374,21 +343,15 @@ public static AutoRoutine fourPieceLeft(
374343
ejector.eject(20).until(ejector.backSensor.negate()),
375344
parallel(
376345
drive.autoAlign(DriveSetpoints.CLOSE_LEFT::getPose),
377-
elevator
378-
.goToSetpoint(
379-
ElevatorSetpoints.AlgaePopLow, intake.backSensor.negate())
380-
.asProxy(),
346+
elevator.goToSetpoint(ElevatorSetpoints.AlgaePopLow).asProxy(),
381347
ejector.eject(30))
382348
.withTimeout(1),
383349
kToHp.cmd())
384350
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
385351

386352
hpToK
387353
.atTime("RaiseElevator")
388-
.onTrue(
389-
elevator
390-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
391-
.until(ejector.backSensor.negate()));
354+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
392355

393356
return routine;
394357
}
@@ -427,10 +390,7 @@ public static AutoRoutine fivePieceLeft(
427390

428391
startToI
429392
.atTime("RaiseElevator")
430-
.onTrue(
431-
elevator
432-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
433-
.until(ejector.backSensor.negate()));
393+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
434394

435395
iToHp
436396
.done()
@@ -463,10 +423,7 @@ public static AutoRoutine fivePieceLeft(
463423

464424
hpToJ
465425
.atTime("RaiseElevator")
466-
.onTrue(
467-
elevator
468-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
469-
.until(ejector.backSensor.negate()));
426+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
470427

471428
hpToL
472429
.done()
@@ -490,10 +447,7 @@ public static AutoRoutine fivePieceLeft(
490447

491448
hpToL
492449
.atTime("RaiseElevator")
493-
.onTrue(
494-
elevator
495-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
496-
.until(ejector.backSensor.negate()));
450+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
497451

498452
hpToK
499453
.done()
@@ -505,21 +459,15 @@ public static AutoRoutine fivePieceLeft(
505459
ejector.eject(20).until(ejector.backSensor.negate()),
506460
parallel(
507461
drive.autoAlign(DriveSetpoints.CLOSE_LEFT::getPose),
508-
elevator
509-
.goToSetpoint(
510-
ElevatorSetpoints.AlgaePopLow, intake.backSensor.negate())
511-
.asProxy(),
462+
elevator.goToSetpoint(ElevatorSetpoints.AlgaePopLow).asProxy(),
512463
ejector.eject(30))
513464
.withTimeout(0.5),
514465
kToHp.cmd())
515466
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
516467

517468
hpToK
518469
.atTime("RaiseElevator")
519-
.onTrue(
520-
elevator
521-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
522-
.until(ejector.backSensor.negate()));
470+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
523471

524472
kToHp
525473
.done()
@@ -532,10 +480,7 @@ public static AutoRoutine fivePieceLeft(
532480

533481
hpToK2
534482
.atTime("RaiseElevator")
535-
.onTrue(
536-
elevator
537-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
538-
.until(ejector.backSensor.negate()));
483+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
539484

540485
hpToK2
541486
.done()
@@ -586,10 +531,7 @@ public static AutoRoutine fivePieceRight(
586531

587532
startToF
588533
.atTime("RaiseElevator")
589-
.onTrue(
590-
elevator
591-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
592-
.until(ejector.backSensor.negate()));
534+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
593535

594536
fToHp
595537
.done()
@@ -622,10 +564,7 @@ public static AutoRoutine fivePieceRight(
622564

623565
hpToE
624566
.atTime("RaiseElevator")
625-
.onTrue(
626-
elevator
627-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
628-
.until(ejector.backSensor.negate()));
567+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
629568

630569
hpToD
631570
.done()
@@ -649,10 +588,7 @@ public static AutoRoutine fivePieceRight(
649588

650589
hpToD
651590
.atTime("RaiseElevator")
652-
.onTrue(
653-
elevator
654-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
655-
.until(ejector.backSensor.negate()));
591+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
656592

657593
hpToC
658594
.done()
@@ -664,21 +600,15 @@ public static AutoRoutine fivePieceRight(
664600
ejector.eject(20).until(ejector.backSensor.negate()),
665601
parallel(
666602
drive.autoAlign(DriveSetpoints.CLOSE_RIGHT::getPose),
667-
elevator
668-
.goToSetpoint(
669-
ElevatorSetpoints.AlgaePopLow, intake.backSensor.negate())
670-
.asProxy(),
603+
elevator.goToSetpoint(ElevatorSetpoints.AlgaePopLow).asProxy(),
671604
ejector.eject(30))
672605
.withTimeout(0.5),
673606
cToHp.cmd())
674607
.withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
675608

676609
hpToC
677610
.atTime("RaiseElevator")
678-
.onTrue(
679-
elevator
680-
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
681-
.until(ejector.backSensor.negate()));
611+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L3).until(ejector.backSensor.negate()));
682612

683613
cToHp
684614
.done()
@@ -691,10 +621,7 @@ public static AutoRoutine fivePieceRight(
691621

692622
hpToC2
693623
.atTime("RaiseElevator")
694-
.onTrue(
695-
elevator
696-
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
697-
.until(ejector.backSensor.negate()));
624+
.onTrue(elevator.goToSetpoint(ElevatorSetpoints.L2).until(ejector.backSensor.negate()));
698625

699626
hpToC2
700627
.done()

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
* (log replay from a file).
1616
*/
1717
public final class Constants {
18-
public static final RobotType robotType = RobotType.COMPBOT;
18+
public static final RobotType robotType = RobotType.SIMBOT;
1919
public static final double ROBOT_X = 0.705;
2020
public static final double ROBOT_Y = 0.730;
2121
public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

0 commit comments

Comments
 (0)