Skip to content

Commit 86e5159

Browse files
TotalTaxAmountTotalTaxAmount
authored
Merge pull request #63 from TotalTaxAmount/dev
All code after arkanasas
2 parents 56cedcd + 6b1e93a commit 86e5159

19 files changed

+380
-236
lines changed

‎build.gradle

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ repositories {
1414
}
1515

1616

17-
def ROBOT_MAIN_CLASS = "frc.robot.Main"
17+
def ROBOT_MAIN_CLASS = "org.blackknights.Main"
1818

1919
// Define my targets (RoboRIO) and artifacts (deployable files)
2020
// This is added by GradleRIO's backing project DeployUtils.

‎src/main/java/org/blackknights/RobotContainer.java

Lines changed: 177 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,15 @@
11
/* Black Knights Robotics (C) 2025 */
22
package org.blackknights;
33

4+
import edu.wpi.first.math.geometry.Pose2d;
5+
import edu.wpi.first.math.geometry.Rotation2d;
6+
import edu.wpi.first.math.geometry.Transform2d;
47
import edu.wpi.first.wpilibj.DriverStation;
58
import edu.wpi.first.wpilibj.GenericHID;
69
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
710
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
811
import edu.wpi.first.wpilibj2.command.*;
12+
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
913
import java.util.function.Supplier;
1014
import org.blackknights.commands.*;
1115
import org.blackknights.constants.ScoringConstants;
@@ -28,8 +32,8 @@ public class RobotContainer {
2832
ButtonBoardSubsystem buttonBoardSubsystem = new ButtonBoardSubsystem(buttonBoard);
2933

3034
// Controllers
31-
Controller primaryController = new Controller(0);
32-
Controller secondaryController = new Controller(1);
35+
CommandXboxController primaryController = new CommandXboxController(0);
36+
CommandXboxController secondaryController = new CommandXboxController(1);
3337

3438
private final NetworkTablesUtils NTTune = NetworkTablesUtils.getTable("debug");
3539

@@ -61,17 +65,33 @@ public RobotContainer() {
6165
SmartDashboard.putData(cqProfiles);
6266
SmartDashboard.putData("Auto Chooser", superSecretMissileTech);
6367

68+
// Autos
6469
superSecretMissileTech.addOption(
65-
"Left",
70+
"LEFT_3",
6671
new SequentialCommandGroup(
6772
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")),
68-
getAutoIntakeCommand(),
73+
getAutoIntakeCommand(IntakeSides.LEFT),
6974
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")),
70-
getAutoIntakeCommand(),
75+
getAutoIntakeCommand(IntakeSides.LEFT),
7176
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4"))));
7277

7378
superSecretMissileTech.addOption(
74-
"One pose", getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")));
79+
"RIGHT_3",
80+
new SequentialCommandGroup(
81+
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")),
82+
getAutoIntakeCommand(IntakeSides.RIGHT),
83+
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")),
84+
getAutoIntakeCommand(IntakeSides.RIGHT),
85+
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4"))));
86+
87+
superSecretMissileTech.addOption(
88+
"CENTER_LEFT",
89+
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4")));
90+
superSecretMissileTech.addOption(
91+
"CENTER_RIGHT",
92+
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4")));
93+
94+
superSecretMissileTech.addOption("INTAKE_TEST", getAutoIntakeCommand(IntakeSides.RIGHT));
7595
}
7696

7797
/** Configure controller bindings */
@@ -82,33 +102,104 @@ private void configureBindings() {
82102
swerveSubsystem.setDefaultCommand(
83103
new DriveCommands(
84104
swerveSubsystem,
85-
() -> primaryController.getLeftY() * 2.5,
86-
() -> primaryController.getLeftX() * 2.5,
87-
() -> -primaryController.getRightX() * Math.PI,
105+
() ->
106+
primaryController.getLeftY()
107+
* ConfigManager.getInstance().get("driver_max_speed", 3.5),
108+
() ->
109+
primaryController.getLeftX()
110+
* ConfigManager.getInstance().get("driver_max_speed", 3.5),
111+
() ->
112+
-primaryController.getRightX()
113+
* Math.toRadians(
114+
ConfigManager.getInstance()
115+
.get("driver_max_speed_rot", 360)),
88116
true,
89117
true));
90118

91-
primaryController.rightBumper.whileTrue(
92-
getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext()));
119+
primaryController
120+
.leftBumper()
121+
.whileTrue(
122+
getPlaceCommand(
123+
() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext()));
93124

94-
primaryController.leftBumper.whileTrue(
95-
new ParallelCommandGroup(
96-
new ElevatorArmCommand(
97-
elevatorSubsystem,
98-
armSubsystem,
99-
() -> ScoringConstants.ScoringHeights.INTAKE),
100-
new IntakeCommand(
101-
intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false)));
125+
primaryController
126+
.rightBumper()
127+
.whileTrue(
128+
new ParallelCommandGroup(
129+
new DriveCommands(
130+
swerveSubsystem,
131+
primaryController::getLeftY,
132+
primaryController::getLeftX,
133+
() -> -primaryController.getRightX() * Math.PI,
134+
true,
135+
true),
136+
new ElevatorArmCommand(
137+
elevatorSubsystem,
138+
armSubsystem,
139+
() -> ScoringConstants.ScoringHeights.INTAKE),
140+
new IntakeCommand(
141+
intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)));
102142

103143
elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem));
104144

145+
primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro()));
146+
147+
secondaryController
148+
.rightStick()
149+
.onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions));
150+
151+
// SECONDARY CONTROLLER
152+
105153
climberSubsystem.setDefaultCommand(
106154
new ClimberCommand(climberSubsystem, secondaryController));
107155

108-
primaryController.dpadDown.whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro()));
156+
secondaryController
157+
.a()
158+
.whileTrue(
159+
new ElevatorArmCommand(
160+
elevatorSubsystem,
161+
armSubsystem,
162+
() -> ScoringConstants.ScoringHeights.L1));
163+
164+
secondaryController
165+
.b()
166+
.whileTrue(
167+
new ElevatorArmCommand(
168+
elevatorSubsystem,
169+
armSubsystem,
170+
() -> ScoringConstants.ScoringHeights.L2));
171+
172+
secondaryController
173+
.x()
174+
.whileTrue(
175+
new ElevatorArmCommand(
176+
elevatorSubsystem,
177+
armSubsystem,
178+
() -> ScoringConstants.ScoringHeights.L3));
179+
180+
secondaryController
181+
.y()
182+
.whileTrue(
183+
new ElevatorArmCommand(
184+
elevatorSubsystem,
185+
armSubsystem,
186+
() -> ScoringConstants.ScoringHeights.L4));
187+
188+
secondaryController
189+
.leftBumper()
190+
.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); //
109191

110-
secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepBackwards()));
111-
secondaryController.rightBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards()));
192+
secondaryController
193+
.rightBumper()
194+
.onTrue(new InstantCommand(() -> coralQueue.stepBackwards()));
195+
196+
secondaryController
197+
.rightTrigger(0.2)
198+
.whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE));
199+
200+
secondaryController
201+
.leftTrigger(0.2)
202+
.whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
112203
}
113204

114205
/** Runs once when the code starts */
@@ -125,7 +216,10 @@ public void robotPeriodic() {
125216

126217
/** Runs ones when enabled in teleop */
127218
public void teleopInit() {
128-
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
219+
buttonBoardSubsystem.bind();
220+
221+
if (cqProfiles.getSelected() != null)
222+
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
129223
}
130224

131225
/**
@@ -171,22 +265,32 @@ private Command getPlaceCommand(
171265
false,
172266
"rough"),
173267
new BaseCommand(elevatorSubsystem, armSubsystem)),
174-
new ParallelCommandGroup(
175-
new SequentialCommandGroup(
176-
new AlignCommand(
268+
new ParallelRaceGroup(
269+
new AlignCommand(
177270
swerveSubsystem,
178271
() -> currentSupplier.get().getPose(),
179272
true,
180-
"fine"),
181-
new IntakeCommand(
182-
intakeSubsystem,
183-
IntakeCommand.IntakeMode.OUTTAKE,
184-
() -> armSubsystem.hasGamePiece())
185-
.withTimeout(2)),
273+
"fine")
274+
.withTimeout(
275+
ConfigManager.getInstance()
276+
.get("align_fine_max_time", 3.0)),
277+
new RunCommand(
278+
() ->
279+
intakeSubsystem.setSpeed(
280+
ConfigManager.getInstance()
281+
.get("intake_slow_voltage", -2.0)),
282+
intakeSubsystem),
186283
new ElevatorArmCommand(
187284
elevatorSubsystem,
188285
armSubsystem,
189-
() -> nextSupplier.get().getHeight())));
286+
() -> currentSupplier.get().getHeight())),
287+
new ParallelRaceGroup(
288+
new ElevatorArmCommand(
289+
elevatorSubsystem,
290+
armSubsystem,
291+
() -> nextSupplier.get().getHeight()),
292+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
293+
.withTimeout(1)));
190294
}
191295

192296
/**
@@ -204,27 +308,52 @@ private Command getLocationPlaceCommand(CoralQueue.CoralPosition position) {
204308
*
205309
* @return The command to goto the feeder
206310
*/
207-
private Command getAutoIntakeCommand() {
208-
return new ParallelDeadlineGroup(
311+
private Command getAutoIntakeCommand(IntakeSides side) {
312+
Pose2d intakePose = getPose2d(side);
313+
314+
Pose2d intakePoseFinal =
315+
intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI)));
316+
317+
return new ParallelRaceGroup(
209318
new SequentialCommandGroup(
210319
new AlignCommand(
211320
swerveSubsystem,
212321
() ->
213-
DriverStation.getAlliance().isPresent()
214-
&& DriverStation.getAlliance().get()
215-
== DriverStation.Alliance.Blue
216-
? ScoringConstants.INTAKE_BLUE
217-
: ScoringConstants.INTAKE_RED,
218-
true,
219-
"rough"),
220-
new IntakeCommand(
221-
intakeSubsystem,
222-
IntakeCommand.IntakeMode.INTAKE,
223-
() -> armSubsystem.hasGamePiece())
224-
.withTimeout(2)),
322+
AlignUtils.getXDistBack(
323+
intakePoseFinal,
324+
ConfigManager.getInstance()
325+
.get("autointake_first_back", 1.0)),
326+
false,
327+
"auto"),
328+
new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")),
225329
new ElevatorArmCommand(
226330
elevatorSubsystem,
227331
armSubsystem,
228-
() -> ScoringConstants.ScoringHeights.INTAKE));
332+
() -> ScoringConstants.ScoringHeights.INTAKE),
333+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
334+
}
335+
336+
private static Pose2d getPose2d(IntakeSides side) {
337+
Pose2d intakePose;
338+
if (DriverStation.getAlliance().isPresent()
339+
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
340+
if (side == IntakeSides.LEFT) {
341+
intakePose = ScoringConstants.INTAKE_BLUE_LEFT;
342+
} else {
343+
intakePose = ScoringConstants.INTAKE_BLUE_RIGHT;
344+
}
345+
} else {
346+
if (side == IntakeSides.LEFT) {
347+
intakePose = ScoringConstants.INTAKE_RED_LEFT;
348+
} else {
349+
intakePose = ScoringConstants.INTAKE_RED_RIGHT;
350+
}
351+
}
352+
return intakePose;
353+
}
354+
355+
private enum IntakeSides {
356+
LEFT,
357+
RIGHT
229358
}
230359
}

‎src/main/java/org/blackknights/commands/AlignCommand.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -252,6 +252,6 @@ public boolean isFinished() {
252252

253253
@Override
254254
public void end(boolean interrupted) {
255-
if (stopWhenFinished) swerveSubsystem.drive(0, 0, 0, false, false, false);
255+
if (stopWhenFinished) swerveSubsystem.zeroVoltage();
256256
}
257257
}

‎src/main/java/org/blackknights/commands/BaseCommand.java

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
import edu.wpi.first.wpilibj2.command.Command;
55
import org.blackknights.subsystems.ArmSubsystem;
66
import org.blackknights.subsystems.ElevatorSubsystem;
7+
import org.blackknights.utils.ConfigManager;
78

89
/** Default command to keep the elevator and arm at rest */
910
public class BaseCommand extends Command {
@@ -30,7 +31,7 @@ public void initialize() {
3031

3132
@Override
3233
public void execute() {
33-
armSubsystem.setPivotAngle(-0.1);
34+
armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1));
3435
if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) {
3536
elevatorSubsystem.holdPosition();
3637
} else {

‎src/main/java/org/blackknights/commands/ClimberCommand.java

Lines changed: 13 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,39 +1,41 @@
11
/* Black Knights Robotics (C) 2025 */
22
package org.blackknights.commands;
33

4+
import edu.wpi.first.math.MathUtil;
45
import edu.wpi.first.wpilibj2.command.Command;
6+
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
57
import org.blackknights.subsystems.ClimberSubsystem;
6-
import org.blackknights.utils.Controller;
78

89
/** Climber command to control the climber */
910
public class ClimberCommand extends Command {
1011
public ClimberSubsystem climberSubsystem;
11-
public Controller controller;
12+
public CommandXboxController controller;
1213

1314
/**
1415
* Command to controller the climber, right now over pure voltage
1516
*
1617
* @param climberSubsystem The instance of {@link ClimberSubsystem}
17-
* @param controller A {@link Controller} to control the climber
18+
* @param controller A {@link CommandXboxController} to control the climber
1819
*/
19-
public ClimberCommand(ClimberSubsystem climberSubsystem, Controller controller) {
20+
public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController controller) {
2021
this.climberSubsystem = climberSubsystem;
2122
this.controller = controller;
2223
addRequirements(climberSubsystem);
2324
}
2425

2526
@Override
2627
public void execute() {
27-
if (controller.dpadDown.getAsBoolean()) {
28-
climberSubsystem.setClimberSpeed(1);
29-
} else if (controller.dpadUp.getAsBoolean()) {
30-
climberSubsystem.setClimberSpeed(-1);
31-
} else if (controller.dpadLeft.getAsBoolean()) {
28+
if (!MathUtil.isNear(controller.getLeftY(), 0.0, 0.1)) {
29+
climberSubsystem.setClimberSpeed(controller.getLeftY());
30+
} else {
31+
climberSubsystem.setClimberSpeed(0);
32+
}
33+
34+
if (controller.povLeft().getAsBoolean()) {
3235
climberSubsystem.setLockSpeed(0.5);
33-
} else if (controller.dpadRight.getAsBoolean()) {
36+
} else if (controller.povRight().getAsBoolean()) {
3437
climberSubsystem.setLockSpeed(-0.5);
3538
} else {
36-
climberSubsystem.setClimberSpeed(0);
3739
climberSubsystem.setLockSpeed(0);
3840
}
3941
}

0 commit comments

Comments
 (0)