Skip to content

Commit cf7b8e6

Browse files
ds12ads12a
committed
reef algae
1 parent b8778d0 commit cf7b8e6

File tree

11 files changed

+158
-113
lines changed

11 files changed

+158
-113
lines changed

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

Lines changed: 63 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
1313
import edu.wpi.first.wpilibj2.command.Command;
1414
import edu.wpi.first.wpilibj2.command.Commands;
15+
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
1516
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
1617
import edu.wpi.first.wpilibj2.command.button.Trigger;
1718
import frc.robot.commands.algae.IntakeAlgaeCommand;
@@ -28,17 +29,15 @@
2829
import frc.robot.commands.elevator.JogElevatorCommand;
2930
import frc.robot.commands.elevator.SetElevatorPositionCommand;
3031
import frc.robot.commands.elevator.ZeroElevatorCommand;
31-
import frc.robot.commands.robotState.AutoScoreReefCommand;
32+
import frc.robot.commands.robotState.AutoReefCycleCommand;
3233
import frc.robot.commands.robotState.FloorAlgaeCommand;
3334
import frc.robot.commands.robotState.HPAlgaeCommand;
3435
import frc.robot.commands.robotState.InterruptAutoCommand;
3536
import frc.robot.commands.robotState.ReefCycleCommand;
3637
import frc.robot.commands.robotState.ScoreAlgaeCommand;
3738
import frc.robot.commands.robotState.SetScoreSideRightCommand;
38-
import frc.robot.commands.robotState.SetScoreSideCommand;
3939
import frc.robot.commands.robotState.SetScoringLevelCommand;
4040
import frc.robot.commands.robotState.StowCommand;
41-
import frc.robot.commands.tagAlign.TagAlignCommand;
4241
import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
4342
import frc.robot.commands.robotState.ToggleAutoCommand;
4443
import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
@@ -67,7 +66,6 @@
6766
import frc.robot.subsystems.led.LEDIO;
6867
import frc.robot.subsystems.led.LEDSubsystem;
6968
import frc.robot.subsystems.robotState.RobotStateSubsystem;
70-
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
7169
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
7270
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
7371
import frc.robot.subsystems.vision.VisionSubsystem;
@@ -177,12 +175,6 @@ private void configureTelemetry() {
177175
}
178176

179177
private void configureDriverBindings() {
180-
// Auto score testing
181-
new JoystickButton(driveJoystick, Button.M_RTRIM_UP.id)
182-
.onTrue(
183-
new AutoScoreReefCommand(
184-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, driveSubsystem));
185-
186178
driveSubsystem.setDefaultCommand(
187179
new DriveTeleopCommand(
188180
() -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
@@ -192,10 +184,18 @@ private void configureDriverBindings() {
192184
new JoystickButton(driveJoystick, Button.SWD.id)
193185
.onTrue(
194186
new StowCommand(
195-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
187+
robotStateSubsystem,
188+
elevatorSubsystem,
189+
coralSubsystem,
190+
biscuitSubsystem,
191+
algaeSubsystem))
196192
.onFalse(
197193
new StowCommand(
198-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
194+
robotStateSubsystem,
195+
elevatorSubsystem,
196+
coralSubsystem,
197+
biscuitSubsystem,
198+
algaeSubsystem));
199199
new JoystickButton(driveJoystick, Button.SWA.id)
200200
.onTrue(new InterruptAutoCommand(robotStateSubsystem))
201201
.onFalse(new InterruptAutoCommand(robotStateSubsystem));
@@ -207,14 +207,26 @@ private void configureDriverBindings() {
207207
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
208208

209209
// other stuff
210+
210211
new JoystickButton(driveJoystick, Button.M_SWH.id)
211212
.onTrue(
212-
new ReefCycleCommand(
213-
robotStateSubsystem,
214-
elevatorSubsystem,
215-
coralSubsystem,
216-
biscuitSubsystem,
217-
algaeSubsystem));
213+
new ConditionalCommand(
214+
new AutoReefCycleCommand(
215+
robotStateSubsystem,
216+
elevatorSubsystem,
217+
coralSubsystem,
218+
driveSubsystem,
219+
tagAlignSubsystem,
220+
biscuitSubsystem,
221+
algaeSubsystem),
222+
new ReefCycleCommand(
223+
robotStateSubsystem,
224+
elevatorSubsystem,
225+
coralSubsystem,
226+
biscuitSubsystem,
227+
algaeSubsystem),
228+
() -> robotStateSubsystem.getIsAutoPlacing()));
229+
218230
new JoystickButton(driveJoystick, Button.M_SWE.id)
219231
.onTrue(
220232
new ScoreAlgaeCommand(
@@ -236,10 +248,6 @@ private void configureDriverBindings() {
236248
}
237249

238250
private void configureOperatorBindings() {
239-
// Auto align testing!
240-
new JoystickButton(xboxController, XboxController.Button.kA.value)
241-
.onTrue(new TagAlignCommand(tagAlignSubsystem, driveSubsystem));
242-
243251
// Set Levels
244252
new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
245253
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
@@ -251,16 +259,44 @@ private void configureOperatorBindings() {
251259
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
252260

253261
// Set scoring side
254-
new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
255-
.onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT));
256-
new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
257-
.onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT));
262+
// new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
263+
// .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT));
264+
// new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
265+
// .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT));
266+
267+
// Move biscuit
268+
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
269+
.onTrue(
270+
new JogBiscuitCommand(
271+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
272+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
273+
new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
274+
.onTrue(
275+
new JogBiscuitCommand(
276+
biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
277+
.onFalse(new HoldBiscuitCommand(biscuitSubsystem));
278+
279+
// Move elevator
280+
new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
281+
.onTrue(
282+
new JogElevatorCommand(
283+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
284+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
285+
new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
286+
.onTrue(
287+
new JogElevatorCommand(
288+
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
289+
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
258290

259291
// Stow
260292
new JoystickButton(xboxController, XboxController.Button.kBack.value)
261293
.onTrue(
262294
new StowCommand(
263-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
295+
robotStateSubsystem,
296+
elevatorSubsystem,
297+
coralSubsystem,
298+
biscuitSubsystem,
299+
algaeSubsystem));
264300

265301
// Algae
266302
new JoystickButton(xboxController, XboxController.Button.kY.value)

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

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
6+
import frc.robot.subsystems.coral.CoralSubsystem;
7+
import frc.robot.subsystems.drive.DriveSubsystem;
8+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
9+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
10+
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
11+
12+
public class AutoReefCycleCommand extends Command {
13+
private RobotStateSubsystem robotStateSubsystem;
14+
private TagAlignSubsystem tagAlignSubsystem;
15+
private DriveSubsystem driveSubsystem;
16+
private boolean scoringCoral;
17+
18+
public AutoReefCycleCommand(
19+
RobotStateSubsystem robotStateSubsystem,
20+
ElevatorSubsystem elevatorSubsystem,
21+
CoralSubsystem coralSubsystem,
22+
DriveSubsystem driveSubsystem,
23+
TagAlignSubsystem tagAlignSubsystem,
24+
BiscuitSubsystem biscuitSubsystem,
25+
AlgaeSubsystem algaeSubsystem) {
26+
this.robotStateSubsystem = robotStateSubsystem;
27+
this.tagAlignSubsystem = tagAlignSubsystem;
28+
this.driveSubsystem = driveSubsystem;
29+
30+
addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem);
31+
}
32+
33+
@Override
34+
public void initialize() {
35+
driveSubsystem.setIgnoreSticks(true);
36+
robotStateSubsystem.toReefAlign();
37+
scoringCoral = robotStateSubsystem.hasCoral();
38+
}
39+
40+
@Override
41+
public void end(boolean interrupted) {
42+
tagAlignSubsystem.terminate();
43+
driveSubsystem.setIgnoreSticks(false);
44+
}
45+
46+
@Override
47+
public boolean isFinished() {
48+
return scoringCoral && !robotStateSubsystem.hasCoral()
49+
|| !scoringCoral && robotStateSubsystem.hasAlgae();
50+
}
51+
}

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

Lines changed: 0 additions & 35 deletions
This file was deleted.

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,13 +4,15 @@
44
import frc.robot.subsystems.algae.AlgaeSubsystem;
55
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
66
import frc.robot.subsystems.coral.CoralSubsystem;
7+
import frc.robot.subsystems.drive.DriveSubsystem;
78
import frc.robot.subsystems.elevator.ElevatorSubsystem;
89
import frc.robot.subsystems.robotState.RobotStateSubsystem;
910
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
1011

1112
public class ReefCycleCommand extends Command {
1213
private RobotStateSubsystem robotStateSubsystem;
1314
private ElevatorSubsystem elevatorSubsystem;
15+
private DriveSubsystem driveSubsystem;
1416
private RobotStates startingRobotState;
1517
private boolean startingElevatorFinished;
1618
private boolean isAutoPlacing;
@@ -23,14 +25,16 @@ public ReefCycleCommand(
2325
AlgaeSubsystem algaeSubsystem) {
2426
this.robotStateSubsystem = robotStateSubsystem;
2527
this.elevatorSubsystem = elevatorSubsystem;
28+
2629
addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem);
2730
}
2831

2932
@Override
3033
public void initialize() {
3134
startingRobotState = robotStateSubsystem.getState();
3235
startingElevatorFinished = elevatorSubsystem.isFinished();
33-
isAutoPlacing = robotStateSubsystem.getIsAuto();
36+
isAutoPlacing = robotStateSubsystem.getIsAutoPlacing();
37+
3438
robotStateSubsystem.toPrepCoral();
3539
}
3640

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

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package frc.robot.commands.robotState;
22

33
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
45
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
56
import frc.robot.subsystems.coral.CoralSubsystem;
67
import frc.robot.subsystems.elevator.ElevatorSubsystem;
@@ -14,9 +15,10 @@ public StowCommand(
1415
RobotStateSubsystem robotState,
1516
ElevatorSubsystem elevatorSubsystem,
1617
CoralSubsystem coralSubsystem,
17-
BiscuitSubsystem biscuitSubsystem) {
18+
BiscuitSubsystem biscuitSubsystem,
19+
AlgaeSubsystem algaeSubsystem) {
1820
this.robotState = robotState;
19-
addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem);
21+
addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem);
2022
}
2123

2224
@Override

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

Lines changed: 3 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,14 @@
44
import frc.robot.subsystems.robotState.RobotStateSubsystem;
55

66
public class ToggleAutoCommand extends InstantCommand {
7-
RobotStateSubsystem robotState;
7+
private RobotStateSubsystem robotStateSubsystem;
88

99
public ToggleAutoCommand(RobotStateSubsystem robotState) {
10-
this.robotState = robotState;
10+
this.robotStateSubsystem = robotState;
1111
}
1212

1313
@Override
1414
public void initialize() {
15-
if (robotState.getIsAuto() == false) {
16-
robotState.setIsAuto(true);
17-
} else {
18-
robotState.setIsAuto(false);
19-
}
15+
robotStateSubsystem.setIsAutoPlacing(!robotStateSubsystem.getIsAutoPlacing());
2016
}
2117
}

‎src/main/java/frc/robot/constants/BiscuitConstants.java‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -40,15 +40,12 @@ public class BiscuitConstants {
4040
public static final Angle kPrestageSetpoint = kStowSetpoint;
4141

4242
// Algae removal
43-
public static final Angle kL2AlgaeSetpoint = Rotations.of(20.848);
43+
public static final Angle kL2AlgaeSetpoint = Rotations.of(24.104);
4444
public static final Angle kL3AlgaeSetpoint = Rotations.of(24.562);
4545

4646
public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint;
4747
public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint;
4848

49-
public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0);
50-
public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0);
51-
5249
// Coral score
5350
public static final Angle kL1CoralSetpoint = kStowSetpoint;
5451
public static final Angle kL2CoralSetpoint = kStowSetpoint;

‎src/main/java/frc/robot/constants/ElevatorConstants.java‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -43,15 +43,12 @@ public class ElevatorConstants {
4343
public static final Angle kStowSetpoint = kFunnelSetpoint;
4444

4545
// Algae removal
46-
public static final Angle kL2AlgaeSetpoint = Rotations.of(2.8677);
46+
public static final Angle kL2AlgaeSetpoint = Rotations.of(6.308);
4747
public static final Angle kL3AlgaeSetpoint = Rotations.of(17.513);
4848

4949
public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint;
5050
public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint;
5151

52-
public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(2.8677);
53-
public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(15.542);
54-
5552
// Coral score
5653
public static final Angle kL1CoralSetpoint = Rotations.of(13.04053);
5754
public static final Angle kL2CoralSetpoint = Rotations.of(21.0786); // 19.62793 -> 21.0786

‎src/main/java/frc/robot/constants/TagServoingConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,10 +37,10 @@ public class TagServoingConstants {
3737
public static final double kNoUpdateMicrosec = 500_000;
3838

3939
// Drive
40-
public static final double kInitialDriveRadius = 1.5;
40+
public static final double kInitialDriveRadius = 1.7; // 1.5
4141
public static final double kStopXDriveRadius =
4242
kInitialDriveRadius; // Should be closer to reef than target pose
43-
public static final double kMinStopXDriveRadius = 1.35;
43+
public static final double kMinStopXDriveRadius = 1.55; // 1.35
4444
public static final double kDriveCloseEnough = 0.1;
4545
public static final double kMinVelX = 0.85;
4646

0 commit comments

Comments
 (0)