Skip to content

Commit 4fe996a

Browse files
TotalTaxAmountTotalTaxAmount
committed
misc: All code after Arkansas
1 parent 4e276f6 commit 4fe996a

File tree

13 files changed

+250
-90
lines changed

13 files changed

+250
-90
lines changed

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

Lines changed: 101 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,9 @@
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;
@@ -62,17 +65,33 @@ public RobotContainer() {
6265
SmartDashboard.putData(cqProfiles);
6366
SmartDashboard.putData("Auto Chooser", superSecretMissileTech);
6467

68+
// Autos
6569
superSecretMissileTech.addOption(
66-
"Left",
70+
"LEFT_3",
6771
new SequentialCommandGroup(
6872
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")),
69-
getAutoIntakeCommand(),
73+
getAutoIntakeCommand(IntakeSides.LEFT),
7074
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")),
71-
getAutoIntakeCommand(),
75+
getAutoIntakeCommand(IntakeSides.LEFT),
7276
getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4"))));
7377

7478
superSecretMissileTech.addOption(
75-
"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));
7695
}
7796

7897
/** Configure controller bindings */
@@ -83,9 +102,9 @@ private void configureBindings() {
83102
swerveSubsystem.setDefaultCommand(
84103
new DriveCommands(
85104
swerveSubsystem,
86-
() -> primaryController.getLeftY() * 2.5,
87-
() -> primaryController.getLeftX() * 2.5,
88-
() -> -primaryController.getRightX() * Math.PI,
105+
() -> primaryController.getLeftY() * ConfigManager.getInstance().get("driver_max_speed", 3.5),
106+
() -> primaryController.getLeftX() * ConfigManager.getInstance().get("driver_max_speed", 3.5),
107+
() -> -primaryController.getRightX() * Math.toRadians(ConfigManager.getInstance().get("driver_max_speed_rot", 360)),
89108
true,
90109
true));
91110

@@ -96,9 +115,16 @@ private void configureBindings() {
96115
() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext()));
97116

98117
primaryController
99-
.leftBumper()
118+
.rightBumper()
100119
.whileTrue(
101120
new ParallelCommandGroup(
121+
new DriveCommands(
122+
swerveSubsystem,
123+
primaryController::getLeftY,
124+
primaryController::getLeftX,
125+
() -> -primaryController.getRightX() * Math.PI,
126+
true,
127+
true),
102128
new ElevatorArmCommand(
103129
elevatorSubsystem,
104130
armSubsystem,
@@ -110,16 +136,15 @@ private void configureBindings() {
110136

111137
primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro()));
112138

139+
secondaryController
140+
.rightStick()
141+
.onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions));
142+
113143
// SECONDARY CONTROLLER
114144

115145
climberSubsystem.setDefaultCommand(
116146
new ClimberCommand(climberSubsystem, secondaryController));
117147

118-
// secondaryController.leftBumper.onTrue(new InstantCommand(() ->
119-
// coralQueue.stepBackwards()));
120-
// secondaryController.rightBumper.onTrue(new InstantCommand(() ->
121-
// coralQueue.stepForwards()));
122-
123148
secondaryController
124149
.a()
125150
.whileTrue(
@@ -154,7 +179,7 @@ private void configureBindings() {
154179

155180
secondaryController
156181
.leftBumper()
157-
.onTrue(new InstantCommand(() -> coralQueue.stepForwards()));
182+
.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); //
158183

159184
secondaryController
160185
.rightBumper()
@@ -183,6 +208,8 @@ public void robotPeriodic() {
183208

184209
/** Runs ones when enabled in teleop */
185210
public void teleopInit() {
211+
buttonBoardSubsystem.bind();
212+
186213
if (cqProfiles.getSelected() != null)
187214
CoralQueue.getInstance().loadProfile(cqProfiles.getSelected());
188215
}
@@ -230,19 +257,32 @@ private Command getPlaceCommand(
230257
false,
231258
"rough"),
232259
new BaseCommand(elevatorSubsystem, armSubsystem)),
233-
new ParallelCommandGroup(
234-
new SequentialCommandGroup(
235-
new AlignCommand(
260+
new ParallelRaceGroup(
261+
new AlignCommand(
236262
swerveSubsystem,
237263
() -> currentSupplier.get().getPose(),
238264
true,
239-
"fine"),
240-
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
241-
.withTimeout(2)),
265+
"fine")
266+
.withTimeout(
267+
ConfigManager.getInstance()
268+
.get("align_fine_max_time", 3.0)),
269+
new RunCommand(
270+
() ->
271+
intakeSubsystem.setSpeed(
272+
ConfigManager.getInstance()
273+
.get("intake_slow_voltage", -2.0)),
274+
intakeSubsystem),
275+
new ElevatorArmCommand(
276+
elevatorSubsystem,
277+
armSubsystem,
278+
() -> currentSupplier.get().getHeight())),
279+
new ParallelRaceGroup(
242280
new ElevatorArmCommand(
243281
elevatorSubsystem,
244282
armSubsystem,
245-
() -> nextSupplier.get().getHeight())));
283+
() -> nextSupplier.get().getHeight()),
284+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
285+
.withTimeout(1)));
246286
}
247287

248288
/**
@@ -260,24 +300,52 @@ private Command getLocationPlaceCommand(CoralQueue.CoralPosition position) {
260300
*
261301
* @return The command to goto the feeder
262302
*/
263-
private Command getAutoIntakeCommand() {
264-
return new ParallelDeadlineGroup(
303+
private Command getAutoIntakeCommand(IntakeSides side) {
304+
Pose2d intakePose = getPose2d(side);
305+
306+
Pose2d intakePoseFinal =
307+
intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI)));
308+
309+
return new ParallelRaceGroup(
265310
new SequentialCommandGroup(
266311
new AlignCommand(
267312
swerveSubsystem,
268313
() ->
269-
DriverStation.getAlliance().isPresent()
270-
&& DriverStation.getAlliance().get()
271-
== DriverStation.Alliance.Blue
272-
? ScoringConstants.INTAKE_BLUE
273-
: ScoringConstants.INTAKE_RED,
274-
true,
275-
"rough"),
276-
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)
277-
.withTimeout(2)),
314+
AlignUtils.getXDistBack(
315+
intakePoseFinal,
316+
ConfigManager.getInstance()
317+
.get("autointake_first_back", 1.0)),
318+
false,
319+
"auto"),
320+
new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")),
278321
new ElevatorArmCommand(
279322
elevatorSubsystem,
280323
armSubsystem,
281-
() -> ScoringConstants.ScoringHeights.INTAKE));
324+
() -> ScoringConstants.ScoringHeights.INTAKE),
325+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
326+
}
327+
328+
private static Pose2d getPose2d(IntakeSides side) {
329+
Pose2d intakePose;
330+
if (DriverStation.getAlliance().isPresent()
331+
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
332+
if (side == IntakeSides.LEFT) {
333+
intakePose = ScoringConstants.INTAKE_BLUE_LEFT;
334+
} else {
335+
intakePose = ScoringConstants.INTAKE_BLUE_RIGHT;
336+
}
337+
} else {
338+
if (side == IntakeSides.LEFT) {
339+
intakePose = ScoringConstants.INTAKE_RED_LEFT;
340+
} else {
341+
intakePose = ScoringConstants.INTAKE_RED_RIGHT;
342+
}
343+
}
344+
return intakePose;
345+
}
346+
347+
private enum IntakeSides {
348+
LEFT,
349+
RIGHT
282350
}
283351
}

‎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: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
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;
56
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
67
import org.blackknights.subsystems.ClimberSubsystem;
@@ -24,16 +25,17 @@ public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController c
2425

2526
@Override
2627
public void execute() {
27-
if (controller.povDown().getAsBoolean()) {
28-
climberSubsystem.setClimberSpeed(1);
29-
} else if (controller.povUp().getAsBoolean()) {
30-
climberSubsystem.setClimberSpeed(-1);
31-
} else if (controller.povLeft().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);
3336
} else if (controller.povRight().getAsBoolean()) {
3437
climberSubsystem.setLockSpeed(-0.5);
3538
} else {
36-
climberSubsystem.setClimberSpeed(0);
3739
climberSubsystem.setLockSpeed(0);
3840
}
3941
}

‎src/main/java/org/blackknights/constants/ArmConstants.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ public class ArmConstants {
1414
public static final TrapezoidProfile.Constraints PIVOT_CONSTRAINTS =
1515
new TrapezoidProfile.Constraints(PIVOT_MAX_VELOCITY, PIVOT_MAX_ACCELERATION);
1616

17-
public static final double PIVOT_ENCODER_OFFSET = 1.581 - 0.092; // 5.167
17+
public static final double PIVOT_ENCODER_OFFSET = 0.0; // 1.581 - 0.092; // 5.167
1818

1919
public static final double PIVOT_KS = 0.0;
2020
public static final double PIVOT_KG = 0.0;

0 commit comments

Comments
 (0)