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

Commit 63849b5

Browse files
fcuellar13fcuellar13
committed
intake hold fixes
1 parent af30583 commit 63849b5

File tree

2 files changed

+40
-19
lines changed

2 files changed

+40
-19
lines changed

‎src/main/java/frc/robot/arm/ArmSubsystem.java‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,9 @@ public void setState(ArmState newState) {
5555

5656
public boolean atGoal() {
5757
return switch (getState()) {
58-
case IDLE, PRE_MATCH_HOMING -> true;
58+
case PRE_MATCH_HOMING -> true;
59+
case IDLE-> MathUtil.isNear(ArmAngle.IDLE, leftMotorAngle, 1)
60+
&& MathUtil.isNear(ArmAngle.IDLE, rightMotorAngle, 1);
5961
case SPEAKER_SHOT ->
6062
MathUtil.isNear(speakerDistanceToAngle.get(distanceToSpeaker), leftMotorAngle, 1)
6163
&& MathUtil.isNear(speakerDistanceToAngle.get(distanceToSpeaker), rightMotorAngle, 1);
@@ -88,7 +90,7 @@ public boolean atGoal() {
8890
case PASS ->
8991
MathUtil.isNear(ArmAngle.PASS, leftMotorAngle, 1)
9092
&& MathUtil.isNear(ArmAngle.PASS, rightMotorAngle, 1);
91-
case HOLD_FOR_INTAKE->true;
93+
case HOLD_FOR_INTAKE -> true;
9294
};
9395
}
9496

@@ -171,7 +173,8 @@ protected void afterTransition(ArmState newState) {
171173
pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS))));
172174
}
173175
case HOLD_FOR_INTAKE -> {
174-
leftMotor.setVoltage(-0.5);;
176+
leftMotor.setVoltage(-0.5);
177+
;
175178
rightMotor.setVoltage(-0.5);
176179
}
177180
default -> {}

‎src/main/java/frc/robot/robot_manager/RobotManager.java‎

Lines changed: 34 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -127,10 +127,8 @@ protected RobotState getNextState(RobotState currentState) {
127127
shooter.atGoal() && arm.atGoal() ? RobotState.PODIUM_SCORING : currentState;
128128

129129
case UNJAM -> currentState;
130-
case BEFORE_INTAKING->
131-
arm.atGoal()?RobotState.INTAKING:currentState;
132-
case BEFORE_INTAKING_ASSIST->
133-
arm.atGoal()?RobotState.INTAKE_ASSIST:currentState;
130+
case BEFORE_INTAKING -> arm.atGoal() ? RobotState.INTAKING : currentState;
131+
case BEFORE_INTAKING_ASSIST -> arm.atGoal() ? RobotState.INTAKE_ASSIST : currentState;
134132

135133
case INTAKING, INTAKE_ASSIST -> {
136134
if (!queuer.hasNote()) {
@@ -434,7 +432,12 @@ public void setConfirmShotActive(boolean newValue) {
434432

435433
public void confirmShotRequest() {
436434
switch (getState()) {
437-
case CLIMBING_1_LINEUP, CLIMBING_2_HANGING, INTAKING, INTAKE_ASSIST,BEFORE_INTAKING
435+
case CLIMBING_1_LINEUP,
436+
CLIMBING_2_HANGING,
437+
INTAKING,
438+
INTAKE_ASSIST,
439+
BEFORE_INTAKING,
440+
BEFORE_INTAKING_ASSIST
438441
// INTAKING_BACK,
439442
// INTAKING_FORWARD_PUSH
440443
-> {}
@@ -457,7 +460,8 @@ public void waitAmpRequest() {
457460
// INTAKING_FORWARD_PUSH,
458461
INTAKING,
459462
INTAKE_ASSIST,
460-
BEFORE_INTAKING -> {}
463+
BEFORE_INTAKING,
464+
BEFORE_INTAKING_ASSIST -> {}
461465
default -> setStateFromRequest(RobotState.AMP_WAITING);
462466
}
463467
}
@@ -471,7 +475,8 @@ public void waitSubwooferRequest() {
471475
// INTAKING_FORWARD_PUSH,
472476
INTAKING,
473477
INTAKE_ASSIST,
474-
BEFORE_INTAKING -> {}
478+
BEFORE_INTAKING,
479+
BEFORE_INTAKING_ASSIST -> {}
475480
default -> setStateFromRequest(RobotState.SUBWOOFER_WAITING);
476481
}
477482
}
@@ -485,7 +490,8 @@ public void waitPodiumRequest() {
485490
// INTAKING_FORWARD_PUSH,
486491
INTAKING,
487492
INTAKE_ASSIST,
488-
BEFORE_INTAKING -> {}
493+
BEFORE_INTAKING,
494+
BEFORE_INTAKING_ASSIST -> {}
489495
default -> setStateFromRequest(RobotState.PODIUM_WAITING);
490496
}
491497
}
@@ -497,7 +503,9 @@ public void waitSpeakerRequest() {
497503
// INTAKING_BACK,
498504
// INTAKING_FORWARD_PUSH,
499505
INTAKING,
500-
INTAKE_ASSIST,BEFORE_INTAKING -> {
506+
INTAKE_ASSIST,
507+
BEFORE_INTAKING,
508+
BEFORE_INTAKING_ASSIST -> {
501509
if (DriverStation.isAutonomous()) {
502510
// Bypass intake checks if we're in auto
503511
setStateFromRequest(RobotState.SPEAKER_WAITING);
@@ -577,7 +585,8 @@ public void preparePassRequest() {
577585
// INTAKING_FORWARD_PUSH,
578586
INTAKING,
579587
INTAKE_ASSIST,
580-
BEFORE_INTAKING -> {}
588+
BEFORE_INTAKING,
589+
BEFORE_INTAKING_ASSIST -> {}
581590
default -> setStateFromRequest(RobotState.PASS_PREPARE_TO_SHOOT);
582591
}
583592
}
@@ -608,7 +617,8 @@ public void prepareSpeakerRequest() {
608617
// INTAKING_FORWARD_PUSH,
609618
INTAKING,
610619
INTAKE_ASSIST,
611-
BEFORE_INTAKING -> {
620+
BEFORE_INTAKING,
621+
BEFORE_INTAKING_ASSIST -> {
612622
if (DriverStation.isAutonomous()) {
613623
// Bypass intake checks if we're in auto
614624
setStateFromRequest(RobotState.SPEAKER_PREPARE_TO_SCORE);
@@ -626,7 +636,8 @@ public void prepareAmpRequest() {
626636
// INTAKING_FORWARD_PUSH,
627637
INTAKING,
628638
INTAKE_ASSIST,
629-
BEFORE_INTAKING -> {}
639+
BEFORE_INTAKING,
640+
BEFORE_INTAKING_ASSIST -> {}
630641
default -> setStateFromRequest(RobotState.AMP_PREPARE_TO_SCORE);
631642
}
632643
}
@@ -639,7 +650,8 @@ public void prepareFeedRequest() {
639650
// INTAKING_FORWARD_PUSH,
640651
INTAKING,
641652
INTAKE_ASSIST,
642-
BEFORE_INTAKING -> {}
653+
BEFORE_INTAKING,
654+
BEFORE_INTAKING_ASSIST -> {}
643655
default -> setStateFromRequest(RobotState.FEEDING_PREPARE_TO_SHOOT);
644656
}
645657
}
@@ -651,7 +663,9 @@ public void waitFeedRequest() {
651663
// INTAKING_BACK,
652664
// INTAKING_FORWARD_PUSH,
653665
INTAKING,
654-
INTAKE_ASSIST,BEFORE_INTAKING -> {}
666+
INTAKE_ASSIST,
667+
BEFORE_INTAKING,
668+
BEFORE_INTAKING_ASSIST -> {}
655669
default -> setStateFromRequest(RobotState.FEEDING_WAITING);
656670
}
657671
}
@@ -677,7 +691,9 @@ public void prepareSubwooferRequest() {
677691
// INTAKING_BACK,
678692
// INTAKING_FORWARD_PUSH,
679693
INTAKING,
680-
INTAKE_ASSIST,BEFORE_INTAKING -> {}
694+
INTAKE_ASSIST,
695+
BEFORE_INTAKING,
696+
BEFORE_INTAKING_ASSIST -> {}
681697
default -> setStateFromRequest(RobotState.SUBWOOFER_PREPARE_TO_SCORE);
682698
}
683699
}
@@ -689,7 +705,9 @@ public void preparePodiumRequest() {
689705
// INTAKING_BACK,
690706
// INTAKING_FORWARD_PUSH,
691707
INTAKING,
692-
INTAKE_ASSIST,BEFORE_INTAKING -> {}
708+
INTAKE_ASSIST,
709+
BEFORE_INTAKING,
710+
BEFORE_INTAKING_ASSIST -> {}
693711
default -> setStateFromRequest(RobotState.PODIUM_PREPARE_TO_SCORE);
694712
}
695713
}

0 commit comments

Comments
 (0)