Skip to content

Commit 6a0e268

Browse files
ds12ads12a
committed
path tweaks, l1 bug fixes, auton bug, bugs
1 parent eedfa93 commit 6a0e268

19 files changed

+277
-203
lines changed

‎src/main/deploy/choreo/bargeToE.traj‎

Lines changed: 156 additions & 152 deletions
Large diffs are not rendered by default.

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

Lines changed: 24 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -461,29 +461,30 @@ private void configureOperatorBindings() {
461461

462462
new Trigger(() -> xboxController.getPOV() != -1).onTrue(new EjectAlgaeCommand(algaeSubsystem));
463463

464-
// Move biscuit
465-
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
466-
.onTrue(
467-
new JogBiscuitCommand(
468-
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
469-
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
470-
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
471-
.onTrue(
472-
new JogBiscuitCommand(
473-
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
474-
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
475-
476-
// Move elevator
477-
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
478-
.onTrue(
479-
new JogElevatorCommand(
480-
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
481-
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
482-
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
483-
.onTrue(
484-
new JogElevatorCommand(
485-
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
486-
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
464+
// // Move biscuit
465+
// new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
466+
// .onTrue(
467+
// new JogBiscuitCommand(
468+
// biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
469+
// .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
470+
// new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
471+
// .onTrue(
472+
// new JogBiscuitCommand(
473+
// biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
474+
// .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
475+
476+
// // Move elevator
477+
// new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
478+
// .onTrue(
479+
// new JogElevatorCommand(
480+
// elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
481+
// .onFalse(new HoldElevatorCommand(elevatorSubsystem));
482+
// new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
483+
// .onTrue(
484+
// new JogElevatorCommand(
485+
// elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown,
486+
// Rotations)))
487+
// .onFalse(new HoldElevatorCommand(elevatorSubsystem));
487488
}
488489

489490
private void configureTestOperatorBindings() {

‎src/main/java/frc/robot/commands/auton/DriveAlgaeWaitAutonServoCommand.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -160,10 +160,10 @@ public void execute() {
160160
double currX = driveSubsystem.getPoseMeters().getX();
161161

162162
if (shouldTransitionToServoing()
163-
&& currX > DriveConstants.kCenterLineX
164-
&& robotStateSubsystem.getAllianceColor() == Alliance.Blue
165-
|| robotStateSubsystem.getAllianceColor() == Alliance.Red
166-
&& currX < DriveConstants.kCenterLineX) {
163+
&& (currX > DriveConstants.kCenterLineX
164+
&& robotStateSubsystem.getAllianceColor() == Alliance.Blue
165+
|| robotStateSubsystem.getAllianceColor() == Alliance.Red
166+
&& currX < DriveConstants.kCenterLineX)) {
167167
isServoing = true;
168168

169169
visionSubsystem.setIsAuto(true);

‎src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ public class AutoReefCycleCommand extends Command {
1616
private TagAlignSubsystem tagAlignSubsystem;
1717
private DriveSubsystem driveSubsystem;
1818
private boolean scoringCoral;
19+
private boolean safeMoveElevator;
1920

2021
public AutoReefCycleCommand(
2122
RobotStateSubsystem robotStateSubsystem,
@@ -34,6 +35,7 @@ public AutoReefCycleCommand(
3435

3536
@Override
3637
public void initialize() {
38+
safeMoveElevator = robotStateSubsystem.safeMoveElevator();
3739
driveSubsystem.setIgnoreSticks(true);
3840
robotStateSubsystem.toReefAlign();
3941
scoringCoral =
@@ -50,7 +52,8 @@ public void end(boolean interrupted) {
5052

5153
@Override
5254
public boolean isFinished() {
53-
return scoringCoral
55+
return !safeMoveElevator
56+
|| scoringCoral
5457
&& (!robotStateSubsystem.hasCoral()
5558
|| robotStateSubsystem.getCoralLevel() == ScoringLevel.L1)
5659
|| !scoringCoral && robotStateSubsystem.hasAlgae();

‎src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
public class FloorAlgaeCommand extends Command {
1111
RobotStateSubsystem robotState;
1212
boolean hasTriedToPickup = false;
13+
private boolean safeMoveElevator;
1314

1415
public FloorAlgaeCommand(
1516
RobotStateSubsystem robotState,
@@ -22,6 +23,7 @@ public FloorAlgaeCommand(
2223

2324
@Override
2425
public void initialize() {
26+
safeMoveElevator = robotState.safeMoveElevator();
2527
hasTriedToPickup = false;
2628
robotState.toAlgaeFloorPickup();
2729
}
@@ -32,6 +34,6 @@ public boolean isFinished() {
3234
|| robotState.getState() == RobotStates.MIC_ALGAE) {
3335
hasTriedToPickup = true;
3436
}
35-
return robotState.getState() == RobotStates.STOW && hasTriedToPickup;
37+
return !safeMoveElevator || robotState.getState() == RobotStates.STOW && hasTriedToPickup;
3638
}
3739
}

‎src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,13 @@
99
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
1010

1111
public class ForceBargeCommand extends Command {
12-
RobotStateSubsystem robotStateSubsystem;
13-
ElevatorSubsystem elevatorSubsystem;
14-
boolean hasTriedToPickup = false;
12+
private RobotStateSubsystem robotStateSubsystem;
13+
private ElevatorSubsystem elevatorSubsystem;
14+
private boolean hasTriedToPickup = false;
1515
private RobotStates startState;
1616
private boolean startingElevatorFinished;
1717
private boolean hasEjectedToBarge;
18+
private boolean safeMoveElevator;
1819

1920
public ForceBargeCommand(
2021
RobotStateSubsystem robotStateSubsystem,
@@ -28,6 +29,7 @@ public ForceBargeCommand(
2829

2930
@Override
3031
public void initialize() {
32+
safeMoveElevator = robotStateSubsystem.safeMoveElevator();
3133
hasEjectedToBarge = false;
3234
startState = robotStateSubsystem.getState();
3335
startingElevatorFinished = elevatorSubsystem.isFinished();
@@ -39,6 +41,9 @@ public void initialize() {
3941

4042
@Override
4143
public boolean isFinished() {
44+
if (!safeMoveElevator) {
45+
return true;
46+
}
4247
if (startState == RobotStates.PROCESSOR_ALGAE
4348
|| startState == RobotStates.BARGE_ALGAE
4449
|| hasEjectedToBarge) {

‎src/main/java/frc/robot/commands/robotState/ForceLowFloorAlgaeCommand.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
1010

1111
public class ForceLowFloorAlgaeCommand extends Command {
12-
RobotStateSubsystem robotState;
12+
private RobotStateSubsystem robotState;
1313
boolean hasTriedToPickup = false;
1414
boolean notSafeElevator = false;
1515

‎src/main/java/frc/robot/commands/robotState/ForceProcessorCommand.java‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,13 @@
99
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
1010

1111
public class ForceProcessorCommand extends Command {
12-
RobotStateSubsystem robotStateSubsystem;
13-
ElevatorSubsystem elevatorSubsystem;
14-
boolean hasTriedToPickup = false;
12+
private RobotStateSubsystem robotStateSubsystem;
13+
private ElevatorSubsystem elevatorSubsystem;
14+
private boolean hasTriedToPickup = false;
1515
private RobotStates startState;
1616
private boolean startingElevatorFinished;
1717
private boolean hasEjectedToBarge;
18+
private boolean safeMoveElevator;
1819

1920
public ForceProcessorCommand(
2021
RobotStateSubsystem robotStateSubsystem,
@@ -28,6 +29,7 @@ public ForceProcessorCommand(
2829

2930
@Override
3031
public void initialize() {
32+
safeMoveElevator = robotStateSubsystem.safeMoveElevator();
3133
hasEjectedToBarge = false;
3234
startState = robotStateSubsystem.getState();
3335
startingElevatorFinished = elevatorSubsystem.isFinished();
@@ -41,6 +43,9 @@ public void initialize() {
4143

4244
@Override
4345
public boolean isFinished() {
46+
if (!safeMoveElevator) {
47+
return true;
48+
}
4449
if (startState == RobotStates.PROCESSOR_ALGAE || hasEjectedToBarge) {
4550
return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
4651
|| robotStateSubsystem.getState() == RobotStates.STOW

‎src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,9 @@
88
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
99

1010
public class HPAlgaeCommand extends Command {
11-
RobotStateSubsystem robotState;
11+
private RobotStateSubsystem robotState;
1212
private boolean hasTriedToGrab = false;
13+
private boolean safeMoveElevator;
1314

1415
public HPAlgaeCommand(
1516
RobotStateSubsystem robotState,
@@ -22,6 +23,7 @@ public HPAlgaeCommand(
2223

2324
@Override
2425
public void initialize() {
26+
safeMoveElevator = robotState.safeMoveElevator();
2527
hasTriedToGrab = false;
2628
robotState.toHpAlgae();
2729
}
@@ -31,6 +33,6 @@ public boolean isFinished() {
3133
if (robotState.getState() == RobotStates.HP_ALGAE) {
3234
hasTriedToGrab = true;
3335
}
34-
return robotState.getState() == RobotStates.STOW && hasTriedToGrab;
36+
return !safeMoveElevator || robotState.getState() == RobotStates.STOW && hasTriedToGrab;
3537
}
3638
}

‎src/main/java/frc/robot/commands/robotState/MicAlgaeCommand.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
public class MicAlgaeCommand extends Command {
1212
private RobotStateSubsystem robotState;
1313
private boolean hasTriedToPickup = false;
14+
private boolean safeMoveElevator;
1415

1516
public MicAlgaeCommand(
1617
RobotStateSubsystem robotState,
@@ -23,6 +24,7 @@ public MicAlgaeCommand(
2324

2425
@Override
2526
public void initialize() {
27+
safeMoveElevator = robotState.safeMoveElevator();
2628
hasTriedToPickup = false;
2729
robotState.setAlgaeHeight(AlgaeHeight.HIGH);
2830
robotState.toAlgaeFloorPickup();
@@ -33,6 +35,6 @@ public boolean isFinished() {
3335
if (robotState.getState() == RobotStates.MIC_ALGAE) {
3436
hasTriedToPickup = true;
3537
}
36-
return robotState.getState() == RobotStates.STOW && hasTriedToPickup;
38+
return !safeMoveElevator || robotState.getState() == RobotStates.STOW && hasTriedToPickup;
3739
}
3840
}

0 commit comments

Comments
 (0)