Skip to content

Commit a4aeb57

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #32 from strykeforce/tag-align
Auto Placing Logic
2 parents 6c593e9 + 8110922 commit a4aeb57

26 files changed

+337
-144
lines changed

‎docs/camsAgreeWithWheels.xlsx‎

8.13 KB
Binary file not shown.

‎docs/getStdFactor Upper.xlsx‎

7.1 KB
Binary file not shown.

‎docs/stdDevDecay.xlsx‎

6.64 KB
Binary file not shown.

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

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,9 @@ public void robotPeriodic() {
5252
}
5353

5454
@Override
55-
public void disabledInit() {}
55+
public void disabledInit() {
56+
m_robotContainer.stopTagAlign();
57+
}
5658

5759
@Override
5860
public void disabledPeriodic() {}

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

Lines changed: 55 additions & 19 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,6 +29,7 @@
2829
import frc.robot.commands.elevator.JogElevatorCommand;
2930
import frc.robot.commands.elevator.SetElevatorPositionCommand;
3031
import frc.robot.commands.elevator.ZeroElevatorCommand;
32+
import frc.robot.commands.robotState.AutoReefCycleCommand;
3133
import frc.robot.commands.robotState.FloorAlgaeCommand;
3234
import frc.robot.commands.robotState.HPAlgaeCommand;
3335
import frc.robot.commands.robotState.InterruptAutoCommand;
@@ -182,10 +184,18 @@ private void configureDriverBindings() {
182184
new JoystickButton(driveJoystick, Button.SWD.id)
183185
.onTrue(
184186
new StowCommand(
185-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
187+
robotStateSubsystem,
188+
elevatorSubsystem,
189+
coralSubsystem,
190+
biscuitSubsystem,
191+
algaeSubsystem))
186192
.onFalse(
187193
new StowCommand(
188-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
194+
robotStateSubsystem,
195+
elevatorSubsystem,
196+
coralSubsystem,
197+
biscuitSubsystem,
198+
algaeSubsystem));
189199
new JoystickButton(driveJoystick, Button.SWA.id)
190200
.onTrue(new InterruptAutoCommand(robotStateSubsystem))
191201
.onFalse(new InterruptAutoCommand(robotStateSubsystem));
@@ -197,14 +207,26 @@ private void configureDriverBindings() {
197207
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
198208

199209
// other stuff
210+
200211
new JoystickButton(driveJoystick, Button.M_SWH.id)
201212
.onTrue(
202-
new ReefCycleCommand(
203-
robotStateSubsystem,
204-
elevatorSubsystem,
205-
coralSubsystem,
206-
biscuitSubsystem,
207-
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+
208230
new JoystickButton(driveJoystick, Button.M_SWE.id)
209231
.onTrue(
210232
new ScoreAlgaeCommand(
@@ -226,6 +248,22 @@ private void configureDriverBindings() {
226248
}
227249

228250
private void configureOperatorBindings() {
251+
// Set Levels
252+
new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
253+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
254+
new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
255+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L2));
256+
new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
257+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L3));
258+
new Trigger(() -> xboxController.getRightTriggerAxis() > RobotConstants.kTriggerDeadband)
259+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
260+
261+
// Set scoring side
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+
229267
// Move biscuit
230268
new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
231269
.onTrue(
@@ -250,21 +288,15 @@ private void configureOperatorBindings() {
250288
elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
251289
.onFalse(new HoldElevatorCommand(elevatorSubsystem));
252290

253-
// Set Levels
254-
new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
255-
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
256-
new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
257-
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L2));
258-
new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
259-
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L3));
260-
new Trigger(() -> xboxController.getRightTriggerAxis() > RobotConstants.kTriggerDeadband)
261-
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
262-
263291
// Stow
264292
new JoystickButton(xboxController, XboxController.Button.kBack.value)
265293
.onTrue(
266294
new StowCommand(
267-
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
295+
robotStateSubsystem,
296+
elevatorSubsystem,
297+
coralSubsystem,
298+
biscuitSubsystem,
299+
algaeSubsystem));
268300

269301
// Algae
270302
new JoystickButton(xboxController, XboxController.Button.kY.value)
@@ -400,4 +432,8 @@ public void configurePitDashboard() {
400432
public Command getAutonomousCommand() {
401433
return Commands.print("No autonomous command configured");
402434
}
435+
436+
public void stopTagAlign() {
437+
tagAlignSubsystem.terminate();
438+
}
403439
}

‎src/main/java/frc/robot/commands/drive/ResetOdometryCommand.java‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
package frc.robot.commands.drive;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.wpilibj2.command.InstantCommand;
5+
import frc.robot.subsystems.drive.DriveSubsystem;
6+
7+
public class ResetOdometryCommand extends InstantCommand {
8+
private DriveSubsystem driveSubsystem;
9+
private Pose2d pose;
10+
11+
public ResetOdometryCommand(DriveSubsystem driveSubsystem, Pose2d pose) {
12+
this.driveSubsystem = driveSubsystem;
13+
this.pose = pose;
14+
}
15+
16+
@Override
17+
public void initialize() {
18+
driveSubsystem.resetOdometry(pose);
19+
}
20+
}

‎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/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.getAutoPlaceOnCycle();
36+
isAutoPlacing = robotStateSubsystem.getIsAutoPlacing();
37+
3438
robotStateSubsystem.toPrepCoral();
3539
}
3640

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ public boolean isFinished() {
4343
} else {
4444
return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
4545
|| robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
46-
|| !robotStateSubsystem.isBargeSafe;
46+
|| !robotStateSubsystem.getIsBargeSafe();
4747
}
4848
}
4949
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ public ScoreReefManualCommand(
2525
public void initialize() {
2626
startingRobotState = robotStateSubsystem.getState();
2727
startingElevatorFinished = elevatorSubsystem.isFinished();
28-
robotStateSubsystem.setAutoPlaceOnCycle(false);
28+
robotStateSubsystem.setIsAutoPlacing(false);
2929
robotStateSubsystem.setGetAlgaeOnCycle(false);
3030
robotStateSubsystem.toPrepCoral();
3131
}

0 commit comments

Comments
 (0)