Skip to content

Commit 0457864

Browse files
TotalTaxAmountTotalTaxAmount
committed
misc: All code after 1619
1 parent a7a9ede commit 0457864

File tree

5 files changed

+136
-46
lines changed

5 files changed

+136
-46
lines changed

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

Lines changed: 75 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import edu.wpi.first.math.geometry.Transform2d;
77
import edu.wpi.first.wpilibj.DriverStation;
88
import edu.wpi.first.wpilibj.GenericHID;
9+
import edu.wpi.first.wpilibj.Timer;
910
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
1011
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1112
import edu.wpi.first.wpilibj2.command.*;
@@ -47,6 +48,12 @@ public class RobotContainer {
4748
Camera.CameraType.PHOTONVISION,
4849
VisionConstants.RIGHT_CAM_TRANSFORM);
4950

51+
private final Camera centerCam =
52+
new Camera(
53+
"centerCam",
54+
Camera.CameraType.PHOTONVISION,
55+
VisionConstants.CENTER_CAM_TRANSFORM);
56+
5057
private final Odometry odometry = Odometry.getInstance();
5158
// Auto Chooser
5259
SendableChooser<Command> superSecretMissileTech = new SendableChooser<>();
@@ -125,20 +132,35 @@ private void configureBindings() {
125132
primaryController
126133
.rightBumper()
127134
.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)));
135+
new SequentialCommandGroup(
136+
new ParallelRaceGroup(
137+
new DriveCommands(
138+
swerveSubsystem,
139+
primaryController::getLeftY,
140+
primaryController::getLeftX,
141+
() -> -primaryController.getRightX() * Math.PI,
142+
true,
143+
true),
144+
new ElevatorArmCommand(
145+
elevatorSubsystem,
146+
armSubsystem,
147+
() -> ScoringConstants.ScoringHeights.INTAKE),
148+
new IntakeCommand(
149+
intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)),
150+
new RunCommand(
151+
() ->
152+
swerveSubsystem.drive(
153+
ConfigManager.getInstance()
154+
.get("back_mps", -1.0),
155+
0.0,
156+
0.0,
157+
false,
158+
false,
159+
false),
160+
swerveSubsystem)
161+
.withTimeout(
162+
ConfigManager.getInstance()
163+
.get("back_time_sec", 0.2))));
142164

143165
elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem));
144166

@@ -206,6 +228,7 @@ private void configureBindings() {
206228
public void robotInit() {
207229
odometry.addCamera(leftCam);
208230
odometry.addCamera(rightCam);
231+
odometry.addCamera(centerCam);
209232
}
210233

211234
/** Runs every 20ms while the robot is on */
@@ -253,6 +276,7 @@ public Command getAutonomousCommand() {
253276
private Command getPlaceCommand(
254277
Supplier<CoralQueue.CoralPosition> currentSupplier,
255278
Supplier<CoralQueue.CoralPosition> nextSupplier) {
279+
256280
return new SequentialCommandGroup(
257281
new ParallelRaceGroup(
258282
new AlignCommand(
@@ -290,7 +314,23 @@ private Command getPlaceCommand(
290314
armSubsystem,
291315
() -> nextSupplier.get().getHeight()),
292316
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
293-
.withTimeout(1)));
317+
.withTimeout(1)),
318+
new ParallelRaceGroup(
319+
new AutoEndCommand(),
320+
new BaseCommand(elevatorSubsystem, armSubsystem),
321+
new RunCommand(
322+
() ->
323+
swerveSubsystem.drive(
324+
ConfigManager.getInstance()
325+
.get("back_mps", -1.0),
326+
0.0,
327+
0.0,
328+
false,
329+
false,
330+
false),
331+
swerveSubsystem)
332+
.withTimeout(
333+
ConfigManager.getInstance().get("back_time_sec", 0.2))));
294334
}
295335

296336
/**
@@ -315,22 +355,13 @@ private Command getAutoIntakeCommand(IntakeSides side) {
315355
intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI)));
316356

317357
return new ParallelRaceGroup(
318-
new SequentialCommandGroup(
319-
new AlignCommand(
320-
swerveSubsystem,
321-
() ->
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")),
358+
new ParallelCommandGroup(
359+
new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "rough"),
360+
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)),
329361
new ElevatorArmCommand(
330362
elevatorSubsystem,
331363
armSubsystem,
332-
() -> ScoringConstants.ScoringHeights.INTAKE),
333-
new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
364+
() -> ScoringConstants.ScoringHeights.INTAKE));
334365
}
335366

336367
private static Pose2d getPose2d(IntakeSides side) {
@@ -356,4 +387,20 @@ private enum IntakeSides {
356387
LEFT,
357388
RIGHT
358389
}
390+
391+
private class AutoEndCommand extends Command {
392+
private double currTime = Timer.getFPGATimestamp() * 1000;
393+
394+
@Override
395+
public void initialize() {
396+
this.currTime = Timer.getFPGATimestamp() * 1000;
397+
}
398+
399+
@Override
400+
public boolean isFinished() {
401+
return DriverStation.isAutonomous()
402+
&& Timer.getFPGATimestamp() * 1000 - this.currTime
403+
> ConfigManager.getInstance().get("auto_place_backup_time_ms", 0.2);
404+
}
405+
}
359406
}

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

Lines changed: 34 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -110,8 +110,41 @@ public AlignCommand(
110110
configManager.get(
111111
String.format("align_%s_rotation_tolerance", this.profile), 1)));
112112

113+
Pose3d robotPose = Odometry.getInstance().getRobotPose();
114+
113115
this.rotationPid.enableContinuousInput(-Math.PI, Math.PI);
114116

117+
this.xAxisPid.reset(
118+
robotPose.getX(),
119+
swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond);
120+
this.yAxisPid.reset(
121+
robotPose.getY(),
122+
swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond);
123+
this.rotationPid.reset(
124+
robotPose.getRotation().getZ(),
125+
swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond);
126+
127+
this.xAxisInfPid.reset(
128+
robotPose.getX(),
129+
swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond);
130+
this.yAxisInfPid.reset(
131+
robotPose.getY(),
132+
swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond);
133+
134+
this.xAxisPid.setGoal(robotPose.getX());
135+
this.yAxisPid.setGoal(robotPose.getY());
136+
this.rotationPid.setGoal(robotPose.getRotation().getZ());
137+
138+
this.xAxisInfPid.setGoal(robotPose.getX());
139+
this.yAxisInfPid.setGoal(robotPose.getY());
140+
141+
this.xAxisPid.calculate(robotPose.getX());
142+
this.yAxisPid.calculate(robotPose.getY());
143+
this.rotationPid.calculate(robotPose.getRotation().getZ());
144+
145+
this.xAxisInfPid.calculate(robotPose.getX());
146+
this.yAxisInfPid.calculate(robotPose.getY());
147+
115148
addRequirements(swerveSubsystem);
116149
}
117150

@@ -212,20 +245,6 @@ public void initialize() {
212245
robotPose.getY(),
213246
swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond);
214247

215-
this.xAxisPid.setGoal(robotPose.getX());
216-
this.yAxisPid.setGoal(robotPose.getY());
217-
this.rotationPid.setGoal(robotPose.getRotation().getZ());
218-
219-
this.xAxisInfPid.setGoal(robotPose.getX());
220-
this.yAxisInfPid.setGoal(robotPose.getY());
221-
222-
this.xAxisPid.calculate(robotPose.getX());
223-
this.yAxisPid.calculate(robotPose.getY());
224-
this.rotationPid.calculate(robotPose.getRotation().getZ());
225-
226-
this.xAxisInfPid.calculate(robotPose.getX());
227-
this.yAxisInfPid.calculate(robotPose.getY());
228-
229248
this.xAxisPid.setGoal(targetPos.getX());
230249
this.yAxisPid.setGoal(targetPos.getY());
231250
this.rotationPid.setGoal(targetPos.getRotation().getRadians());
@@ -286,8 +305,7 @@ public void execute() {
286305
? 0
287306
: Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1));
288307

289-
if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2)
290-
&& !stopWhenFinished) {
308+
if (!stopWhenFinished) {
291309
this.finalX = infX;
292310
this.finalY = infY;
293311
}

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

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,9 @@ public void initialize() {
3232
@Override
3333
public void execute() {
3434
armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1));
35-
if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) {
35+
if (armSubsystem.getPivotAngle() <= -Math.PI / 4
36+
|| armSubsystem.getPivotAngle()
37+
>= ConfigManager.getInstance().get("arm_movement_max", 0.2)) {
3638
elevatorSubsystem.holdPosition();
3739
} else {
3840
elevatorSubsystem.zeroElevator();

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

Lines changed: 13 additions & 1 deletion
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.wpilibj.Timer;
45
import edu.wpi.first.wpilibj2.command.Command;
56
import org.blackknights.subsystems.IntakeSubsystem;
67
import org.blackknights.utils.ConfigManager;
@@ -10,6 +11,8 @@ public class IntakeCommand extends Command {
1011
private final IntakeSubsystem intakeSubsystem;
1112
private final IntakeMode mode;
1213

14+
private double currentTime;
15+
1316
/**
1417
* Create a new intake command
1518
*
@@ -22,6 +25,11 @@ public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) {
2225
addRequirements(intakeSubsystem);
2326
}
2427

28+
@Override
29+
public void initialize() {
30+
this.currentTime = Timer.getFPGATimestamp() * 1000;
31+
}
32+
2533
@Override
2634
public void execute() {
2735
switch (mode) {
@@ -47,7 +55,11 @@ public void end(boolean interrupted) {
4755

4856
@Override
4957
public boolean isFinished() {
50-
return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak();
58+
return (mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak())
59+
|| (mode.equals(IntakeMode.OUTTAKE)
60+
&& !intakeSubsystem.getLinebreak()
61+
&& Timer.getFPGATimestamp() * 1000 - this.currentTime
62+
> ConfigManager.getInstance().get("outtake_wait_time_ms", 40));
5163
}
5264

5365
/** Enum of the different intake modes */

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

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,4 +31,15 @@ public class VisionConstants {
3131
0.0,
3232
Math.toRadians(
3333
ConfigManager.getInstance().get("right_cam_angle", 10.0))));
34+
35+
public static final Transform3d CENTER_CAM_TRANSFORM =
36+
new Transform3d(
37+
0.1, // 0.341122 0.3832
38+
0.0,
39+
0.2040382,
40+
new Rotation3d(
41+
0.0,
42+
Math.toRadians(
43+
ConfigManager.getInstance().get("center_cam_pitch", 45.0)),
44+
0.0));
3445
}

0 commit comments

Comments
 (0)