Skip to content

Commit 863058c

Browse files
blaze-developerblaze-developer
committed
init command
1 parent 7f7d7a2 commit 863058c

5 files changed

Lines changed: 60 additions & 52 deletions

File tree

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

Lines changed: 4 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
import frc.robot.commands.misc.StateCmd;
4242
import frc.robot.generated.CompTunerConstants;
4343
import frc.robot.subsystems.drive.Drive;
44-
import frc.robot.subsystems.vision.MotionTrackerVision;
44+
import frc.robot.subsystems.vision.VisionConstants;
4545
import frc.simulator.engine.SimulationEngine;
4646

4747
/**
@@ -215,27 +215,18 @@ public void disabledPeriodic() {
215215
if(!hasSetupAutos && DriverStation.getAlliance().isPresent()) {
216216
robotContainer.setupAutos();
217217
hasSetupAutos = true;
218-
}
219-
220-
218+
}
221219

222-
if (hasSetupAutos) {
220+
if (hasSetupAutos && !VisionConstants.useQuest) {
223221
Command cmd = robotContainer.getAutonomousCommand();
224222
if (cmd != null) {
225223
AutoModeBaseCmd currentAuto = (AutoModeBaseCmd) cmd;
226224
if (currentAuto != null) {
227-
Drive drivebase = RobotContainer.getInstance().drivebase() ;
228-
MotionTrackerVision quest = RobotContainer.getInstance().quest();
225+
Drive drivebase = RobotContainer.getInstance().drivebase();
229226
Pose2d autopose = currentAuto.getStartingPose() ;
230227
if (appliedAuto == null || appliedAuto != currentAuto) { // Changing the Auto Mode
231-
Logger.recordOutput("automode/name", currentAuto.getName()) ;
232-
Logger.recordOutput("automode/pose", autopose) ;
233-
Logger.recordOutput("poseinit/setting", "Robot Pose");
234228
drivebase.setPose(autopose);
235229
appliedAuto = currentAuto;
236-
} else { // Initializing the Quest Repeatedly
237-
quest.setPose(drivebase.getPose());
238-
Logger.recordOutput("poseinit/setting", "Quest Pose");
239230
}
240231
}
241232
}

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

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -409,7 +409,12 @@ private RobotContainer() {
409409
// Configure the button bindings
410410
configureDriveBindings();
411411
configureButtonBindings();
412-
configureTestModeBindings() ;
412+
configureTestModeBindings();
413+
414+
// Zero Questnav
415+
if (VisionConstants.useQuest) {
416+
questnav_.zero(drivebase_, vision_).schedule();
417+
}
413418

414419
if (Constants.getRobot() != RobotType.SIMBOT) {
415420
manipulator_.setDefaultCommand(new CalibrateCmd(manipulator_));
@@ -420,10 +425,6 @@ public Drive drivebase() {
420425
return drivebase_;
421426
}
422427

423-
public MotionTrackerVision quest() {
424-
return questnav_;
425-
}
426-
427428
public XeroGamepad gamepad() {
428429
return gamepad_;
429430
}
@@ -600,6 +601,9 @@ private void configureDriveBindings() {
600601
// Reset gyro to 0° when Y & B button is pressed
601602
gamepad_.y().and(gamepad_.b()).onTrue(drivebase_.resetGyroCmd());
602603

604+
// ReInitialize Questnav when A & X buttons are pressed
605+
gamepad_.a().and(gamepad_.x()).onTrue(questnav_.zero(drivebase_, vision_));
606+
603607
gamepad_.rightTrigger().onTrue(new ExecuteRobotActionCmd(brain_));
604608
}
605609

‎src/main/java/frc/robot/subsystems/vision/AprilTagVision.java‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,8 @@ public static enum IntegrationBehavior {
4141

4242
private Distance maxTagDistance = kDefaultDistanceFilter ;
4343

44+
private int tagCount = 0; // The amount of tags seen in this robot loop
45+
4446
public AprilTagVision(PoseEstimateConsumer poseEstimateConsumer, CameraIO... io) {
4547
poseEstimateConsumer_ = poseEstimateConsumer;
4648

@@ -62,6 +64,10 @@ public AprilTagVision(PoseEstimateConsumer poseEstimateConsumer, CameraIO... io)
6264
}
6365
}
6466

67+
public int getTagCount() {
68+
return tagCount;
69+
}
70+
6571
public void setTagFilterDistance(Distance d) {
6672
maxTagDistance = (d == null) ? kDefaultDistanceFilter : d ;
6773
}
@@ -94,6 +100,8 @@ public void periodic() {
94100
ArrayList<Pose3d> summaryTagPoses = new ArrayList<>();
95101
ArrayList<PoseEstimation> poseEstimates = new ArrayList<>();
96102

103+
tagCount = 0;
104+
97105
// Iterate cameras for logging and pose estimations.
98106
for (int cam = 0; cam < io_.length; cam++) {
99107

@@ -107,6 +115,7 @@ public void periodic() {
107115

108116
// Loop through visible tags.
109117
for (Fiducial fid : inputs_[cam].fiducials) {
118+
tagCount++;
110119
Optional<Pose3d> pose = FieldConstants.layout.getTagPose(fid.id());
111120
if (pose.isPresent()) {
112121
tagPoses.add(pose.get());

‎src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java‎

Lines changed: 36 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,7 @@
44
import static edu.wpi.first.units.Units.Centimeters;
55
import static edu.wpi.first.units.Units.Degrees;
66
import static edu.wpi.first.units.Units.Meters;
7-
8-
import java.util.Optional;
9-
import java.util.function.Consumer;
10-
import java.util.function.Supplier;
7+
import static edu.wpi.first.units.Units.Seconds;
118

129
import org.littletonrobotics.junction.Logger;
1310

@@ -19,12 +16,14 @@
1916
import edu.wpi.first.math.numbers.N1;
2017
import edu.wpi.first.math.numbers.N3;
2118
import edu.wpi.first.wpilibj.Alert;
22-
import edu.wpi.first.wpilibj.DriverStation;
2319
import edu.wpi.first.wpilibj.Alert.AlertType;
20+
import edu.wpi.first.wpilibj.DriverStation;
2421
import edu.wpi.first.wpilibj.DriverStation.Alliance;
2522
import edu.wpi.first.wpilibj.RobotState;
26-
import edu.wpi.first.wpilibj.Timer;
23+
import edu.wpi.first.wpilibj2.command.Command;
24+
import edu.wpi.first.wpilibj2.command.Commands;
2725
import edu.wpi.first.wpilibj2.command.SubsystemBase;
26+
import frc.robot.subsystems.drive.Drive;
2827
import gg.questnav.questnav.PoseFrame;
2928

3029
public class MotionTrackerVision extends SubsystemBase {
@@ -34,7 +33,7 @@ public class MotionTrackerVision extends SubsystemBase {
3433
0.0001,
3534
0.0001
3635
);
37-
36+
3837
private boolean useQuestNav = false;
3938
private static final boolean calibration = false;
4039
private static final boolean useQueuedFrames = true;
@@ -53,22 +52,42 @@ public class MotionTrackerVision extends SubsystemBase {
5352
private final TrackerInputsAutoLogged inputs_;
5453

5554
private final Alert disconnectedAlert_ = new Alert("Motion Tracker Disconnected!", AlertType.kError);
56-
5755
private final Alert lowBatteryAlert_ = new Alert("Quest battery is low!", AlertType.kWarning);
56+
private final Alert notInitedAlert_ = new Alert("The Quest is not initialized or initializing!!", AlertType.kWarning);
57+
private final Alert initAlert_ = new Alert("Quest has been initialized!", AlertType.kInfo);
5858

5959
private final PoseEstimateConsumer estimateConsumer_;
60-
private final Consumer<Pose2d> poseSetter_;
6160

6261
// Whether or not the quest has been zeroed to the field.
63-
private boolean zeroing = false;
64-
private Double zeroStartTime = 0.0;
6562
private boolean zeroed = false;
6663

67-
public MotionTrackerVision(TrackerIO io, PoseEstimateConsumer estimateConsumer, Consumer<Pose2d> poseSetter) {
64+
public MotionTrackerVision(
65+
TrackerIO io,
66+
PoseEstimateConsumer estimateConsumer
67+
) {
6868
io_ = io;
6969
inputs_ = new TrackerInputsAutoLogged();
7070
estimateConsumer_ = estimateConsumer;
71-
poseSetter_ = poseSetter;
71+
}
72+
73+
public Command zero(Drive drive, AprilTagVision vision) {
74+
return Commands.sequence(
75+
runOnce(() -> zeroed = false),
76+
Commands.waitUntil(() -> DriverStation.getAlliance().isPresent()),
77+
drive.runOnce(() ->
78+
drive.setPose(
79+
DriverStation.getAlliance().orElseThrow() == Alliance.Blue
80+
? new Pose2d(0, 0, Rotation2d.k180deg)
81+
: Pose2d.kZero
82+
)
83+
),
84+
Commands.waitTime(Seconds.of(0.5)),
85+
Commands.waitUntil(() -> vision.getTagCount() > 0),
86+
runOnce(() -> {
87+
setPose(drive.getPose());
88+
zeroed = true;
89+
})
90+
);
7291
}
7392

7493
@Override
@@ -78,32 +97,15 @@ public void periodic() {
7897

7998
boolean disconnected = !inputs_.connected;
8099
disconnectedAlert_.set(disconnected);
81-
82100
lowBatteryAlert_.set(inputs_.batteryPercent < 20);
83-
84-
if (!zeroing) {
85-
Optional<Alliance> alliance = DriverStation.getAlliance();
86-
87-
if (alliance.isPresent()) {
88-
poseSetter_.accept(
89-
alliance.orElseThrow() == Alliance.Red
90-
? Pose2d.kZero
91-
: new Pose2d(0, 0, Rotation2d.k180deg)
92-
);
93-
94-
zeroing = true;
95-
zeroStartTime = Timer.getTimestamp();
96-
}
97-
} else {
98-
// The pose has been set and we are zeroing
99-
100-
101-
}
101+
notInitedAlert_.set(!zeroed);
102+
initAlert_.set(zeroed);
102103

103104
if (
104105
disconnected ||
105106
!inputs_.isTracking ||
106107
RobotState.isDisabled() ||
108+
!zeroed ||
107109
!useQuestNav
108110
) return;
109111

@@ -123,7 +125,7 @@ public void periodic() {
123125
}
124126
}
125127

126-
public void setPose(Pose2d pose) {
128+
private void setPose(Pose2d pose) {
127129
io_.setPose(pose.transformBy(robotToQuest));
128130
}
129131

‎src/main/java/frc/robot/subsystems/vision/VisionConstants.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010

1111
public class VisionConstants {
1212

13+
public static final boolean useQuest = true;
14+
1315
// Limelight Names
1416
public static final String frontLimelightName = "limelight-front";
1517
public static final String backLimelightName = "limelight-back";

0 commit comments

Comments
 (0)