Skip to content

Commit 2bdd2c0

Browse files
blaze-developersjcbulldog
blaze-developer
and
sjcbulldog
authored
Add QuestNav implementations of Vision (#172)
* Questnav * questnav * Quest * Spelling * questnav * questnav * Clear queues * fix npe * Quest offsets and vendordep * use no offset and fix imports & update questnav * changes from the mainline * Merge from * created a single path automode * work on path following[ * updated on odom testing * test work from last saturday * test paths * Worked on tuning odometry and the curved path to be more accurate * speed up path and add constraint zones * Update * Undo * Questnav Zeroing * quest updates * Uncomment * Fix error * Final changes before merge * Remove Paths * undo onepath stuff * Fix error * Fix pp config --------- Co-authored-by: Butch Griffin <butchg@comcast.net>
1 parent 4ed5531 commit 2bdd2c0

14 files changed

Lines changed: 234 additions & 55 deletions

File tree

‎src/main/deploy/pathplanner/paths/ThreeCoral1.path‎

100644100755
Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,4 +51,4 @@
5151
"rotation": 180.0
5252
},
5353
"useDefaultConstraints": false
54-
}
54+
}

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

Lines changed: 18 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +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;
4445
import frc.simulator.engine.SimulationEngine;
4546

4647
/**
@@ -56,7 +57,7 @@ public class Robot extends LoggedRobot {
5657
private RobotContainer robotContainer;
5758

5859
private boolean hasSetupAutos = false;
59-
private AutoModeBaseCmd auto_cmd_ = null ;
60+
private AutoModeBaseCmd appliedAuto = null ;
6061

6162
public Robot() throws RuntimeException {
6263
//
@@ -216,21 +217,28 @@ public void disabledPeriodic() {
216217
hasSetupAutos = true;
217218
}
218219

220+
221+
219222
if (hasSetupAutos) {
220223
Command cmd = robotContainer.getAutonomousCommand();
221224
if (cmd != null) {
222-
AutoModeBaseCmd autoCmd = (AutoModeBaseCmd) cmd;
223-
if (autoCmd != null) {
224-
Drive d = RobotContainer.getInstance().drivebase() ;
225-
Pose2d autopose = autoCmd.getStartingPose() ;
226-
if (auto_cmd_ == null || auto_cmd_ != autoCmd) {
227-
Logger.recordOutput("automode/name", autoCmd.getName()) ;
225+
AutoModeBaseCmd currentAuto = (AutoModeBaseCmd) cmd;
226+
if (currentAuto != null) {
227+
Drive drivebase = RobotContainer.getInstance().drivebase() ;
228+
MotionTrackerVision quest = RobotContainer.getInstance().quest();
229+
Pose2d autopose = currentAuto.getStartingPose() ;
230+
if (appliedAuto == null || appliedAuto != currentAuto) { // Changing the Auto Mode
231+
Logger.recordOutput("automode/name", currentAuto.getName()) ;
228232
Logger.recordOutput("automode/pose", autopose) ;
229-
d.setPose(autopose) ;
230-
auto_cmd_ = autoCmd ;
233+
Logger.recordOutput("poseinit/setting", "Robot Pose");
234+
drivebase.setPose(autopose);
235+
appliedAuto = currentAuto;
236+
} else { // Initializing the Quest Repeatedly
237+
quest.setPose(drivebase.getPose());
238+
Logger.recordOutput("poseinit/setting", "Quest Pose");
231239
}
232240
}
233-
}
241+
}
234242
}
235243
}
236244

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

Lines changed: 33 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
import org.xerosw.util.MessageLogger;
3131
import org.xerosw.util.MessageType;
3232

33+
import edu.wpi.first.math.geometry.Pose2d;
3334
import edu.wpi.first.units.measure.Angle;
3435
import edu.wpi.first.units.measure.Distance;
3536
import edu.wpi.first.wpilibj.RobotState;
@@ -89,8 +90,9 @@
8990
import frc.robot.subsystems.vision.AprilTagVision;
9091
import frc.robot.subsystems.vision.CameraIO;
9192
import frc.robot.subsystems.vision.CameraIOLimelight4;
92-
import frc.robot.subsystems.vision.CameraIOPhotonSim;
93-
import frc.robot.subsystems.vision.PoseEstimateConsumer;
93+
import frc.robot.subsystems.vision.MotionTrackerVision;
94+
import frc.robot.subsystems.vision.TrackerIO;
95+
import frc.robot.subsystems.vision.TrackerIOQuest;
9496
import frc.robot.subsystems.vision.VisionConstants;
9597
import frc.robot.util.Mechanism3d;
9698
import frc.robot.util.ReefUtil;
@@ -123,6 +125,7 @@ public static RobotContainer getInstance() {
123125
// Subsystems
124126
private Drive drivebase_;
125127
private AprilTagVision vision_;
128+
private MotionTrackerVision questnav_;
126129
private OISubsystem oi_;
127130
private ManipulatorSubsystem manipulator_;
128131
private GrabberSubsystem grabber_;
@@ -160,19 +163,24 @@ private RobotContainer() {
160163
if (Constants.getMode() != Mode.REPLAY) {
161164
switch (Constants.getRobot()) {
162165
case COMPETITION:
166+
163167
drivebase_ = new Drive(
164-
new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus),
165-
ModuleIOTalonFX::new,
166-
CompTunerConstants.FrontLeft,
167-
CompTunerConstants.FrontRight,
168-
CompTunerConstants.BackLeft,
169-
CompTunerConstants.BackRight,
170-
CompTunerConstants.kSpeedAt12Volts);
171-
168+
new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus),
169+
ModuleIOTalonFX::new,
170+
CompTunerConstants.FrontLeft,
171+
CompTunerConstants.FrontRight,
172+
CompTunerConstants.BackLeft,
173+
CompTunerConstants.BackRight,
174+
CompTunerConstants.kSpeedAt12Volts
175+
);
176+
177+
questnav_ = new MotionTrackerVision(new TrackerIOQuest(), drivebase_::addVisionMeasurement);
178+
172179
vision_ = new AprilTagVision(
173180
drivebase_::addVisionMeasurement,
174181
new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation)
175182
);
183+
// vision_.setTagFilterDistance(Meters.of(1.2));
176184

177185
try {
178186
manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware());
@@ -257,14 +265,6 @@ private RobotContainer() {
257265
CompTunerConstants.BackRight,
258266
CompTunerConstants.kSpeedAt12Volts);
259267

260-
vision_ = new AprilTagVision(
261-
PoseEstimateConsumer.ignore(),
262-
new CameraIOPhotonSim("Front", VisionConstants.frontTransform,
263-
drivebase_::getPose, true),
264-
new CameraIOPhotonSim("Back", VisionConstants.backTransform,
265-
drivebase_::getPose, true)
266-
);
267-
268268
try {
269269
manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware());
270270
} catch (Exception ex) {
@@ -327,7 +327,7 @@ private RobotContainer() {
327327

328328
if (vision_ == null) {
329329
int numCams = switch (Constants.getRobot()) {
330-
default -> 3;
330+
default -> 1;
331331
};
332332

333333
CameraIO[] cams = new CameraIO[numCams];
@@ -339,6 +339,10 @@ private RobotContainer() {
339339
cams);
340340
}
341341

342+
if (questnav_ == null) {
343+
questnav_ = new MotionTrackerVision(new TrackerIO() {}, drivebase_::addVisionMeasurement);
344+
}
345+
342346
if (manipulator_ == null) {
343347
manipulator_ = new ManipulatorSubsystem(new ManipulatorIO() {
344348
});
@@ -369,7 +373,7 @@ private RobotContainer() {
369373
() -> -gamepad_.getLeftY(),
370374
() -> -gamepad_.getLeftX(),
371375
() -> -gamepad_.getRightX()
372-
);
376+
);
373377

374378
// Shuffleboard Tabs
375379
ShuffleboardTab autonomousTab = Shuffleboard.getTab("Autonomous");
@@ -416,6 +420,10 @@ public Drive drivebase() {
416420
return drivebase_;
417421
}
418422

423+
public MotionTrackerVision quest() {
424+
return questnav_;
425+
}
426+
419427
public XeroGamepad gamepad() {
420428
return gamepad_;
421429
}
@@ -456,6 +464,8 @@ public void setupAutos() {
456464
odometryTest.addCommands(DriveCommands.initialFollowPathCommand(drivebase_, "Odom Test"));
457465

458466
autoChooser_.addOption("Odom Test (aka Kachow)", odometryTest);
467+
468+
autoChooser_.addOption("Zero Pose Reset", new AutoModeBaseCmd("Zero Pose", Pose2d.kZero));
459469
}
460470

461471
private void subsystemCreateException(Exception ex) {
@@ -599,28 +609,11 @@ private void configureDriveBindings() {
599609
* @return the command to run in autonomous
600610
*/
601611
public Command getAutonomousCommand() {
602-
Command ret = null;
603-
604612
if (Robot.useXeroSimulator()) {
605-
//
606-
// In the Xero simulator, set the auto mode you want to run
607-
// Note: the auto used here must match the simulation stimulus file set in the
608-
// Robot.java file.
609-
//
610-
611-
// ret = AutoCommands.oneCoralAuto(brain_, drivebase_, manipulator_, grabber_) ;
612-
ret = AutoCommands.threeCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true) ;
613-
// ret = AutoCommands.oneCoralOneAlgaeAuto(brain_, drivebase_, manipulator_, grabber_) ;
614-
// ret = AutoCommands.twoCoralCenterAuto(brain_, drivebase_, manipulator_, grabber_, funnel_, true);
615-
616-
// Command autoChosen = autoChooser_.get();
617-
// ret = autoChosen != null ? autoChosen : tuningChooser_.get();
618-
619-
} else {
620-
Command autoChosen = autoChooser_.get();
621-
ret = autoChosen != null ? autoChosen : tuningChooser_.get();
613+
return AutoCommands.threeCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true);
622614
}
623615

624-
return ret;
616+
Command autoChosen = autoChooser_.get();
617+
return autoChosen != null ? autoChosen : tuningChooser_.get();
625618
}
626619
}

‎src/main/java/frc/robot/subsystems/drive/Drive.java‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -280,6 +280,10 @@ public void periodic() {
280280

281281
ChassisSpeeds spd = getChassisSpeeds() ;
282282
Logger.recordOutput("drive/velocity", Math.hypot(spd.vxMetersPerSecond, spd.vyMetersPerSecond)) ;
283+
284+
Pose2d pose = getPose();
285+
Logger.recordOutput("Odometry/Individual/X", pose.getX());
286+
Logger.recordOutput("Odometry/Individual/Y", pose.getY());
283287
}
284288

285289
/**
@@ -472,8 +476,7 @@ public void addVisionMeasurement(
472476
double timestampSeconds,
473477
Matrix<N3, N1> visionMeasurementStdDevs) {
474478
Logger.recordOutput("Odometry/VisionMeasurement", visionRobotPoseMeters);
475-
poseEstimator.addVisionMeasurement(
476-
visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
479+
poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
477480
}
478481

479482
/** Returns the maximum linear speed in meters per sec. */

‎src/main/java/frc/robot/subsystems/drive/GyroIO.java‎

100644100755
File mode changed.

‎src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
import com.ctre.phoenix6.BaseStatusSignal;
1919
import com.ctre.phoenix6.CANBus;
20+
import com.ctre.phoenix6.StatusCode;
2021
import com.ctre.phoenix6.StatusSignal;
2122
import com.ctre.phoenix6.configs.GyroTrimConfigs;
2223
import com.ctre.phoenix6.configs.Pigeon2Configuration;
@@ -52,7 +53,7 @@ public GyroIOPigeon2(int Pigeon2Id, CANBus bus) {
5253

5354
@Override
5455
public void updateInputs(GyroIOInputs inputs) {
55-
inputs.connected = BaseStatusSignal.refreshAll(yaw).isOK() ;
56+
inputs.connected = BaseStatusSignal.refreshAll(yaw).equals(StatusCode.OK);
5657
inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
5758

5859
inputs.odometryYawTimestamps =

‎src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java‎

100644100755
File mode changed.

‎src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOHardware.java‎

100644100755
File mode changed.

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

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
2+
package frc.robot.subsystems.vision;
3+
4+
import static edu.wpi.first.units.Units.Degrees;
5+
import static edu.wpi.first.units.Units.Meters;
6+
7+
import org.littletonrobotics.junction.Logger;
8+
9+
import edu.wpi.first.math.Matrix;
10+
import edu.wpi.first.math.VecBuilder;
11+
import edu.wpi.first.math.geometry.Pose2d;
12+
import edu.wpi.first.math.geometry.Rotation2d;
13+
import edu.wpi.first.math.geometry.Transform2d;
14+
import edu.wpi.first.math.numbers.N1;
15+
import edu.wpi.first.math.numbers.N3;
16+
import edu.wpi.first.wpilibj.Alert;
17+
import edu.wpi.first.wpilibj.Alert.AlertType;
18+
import edu.wpi.first.wpilibj.RobotState;
19+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
20+
import gg.questnav.questnav.PoseFrame;
21+
22+
public class MotionTrackerVision extends SubsystemBase {
23+
24+
private static final Matrix<N3, N1> stdDevs = VecBuilder.fill(
25+
0.01,
26+
0.01,
27+
0.01
28+
);
29+
30+
private boolean useQuestNav = false;
31+
private static final boolean calibration = false;
32+
private static final boolean useQueuedFrames = true;
33+
34+
private static final Transform2d robotToQuest = new Transform2d(
35+
!calibration ? Meters.of(0.199) : Meters.zero(),
36+
!calibration ? Meters.of(0.282) : Meters.zero(),
37+
new Rotation2d(Degrees.of(45))
38+
);
39+
40+
// x: -0.216 y: -0.261
41+
42+
// time: 565-593
43+
// time: 794-831
44+
private final TrackerIO io_;
45+
private final TrackerInputsAutoLogged inputs_;
46+
47+
private final Alert disconnectedAlert_ = new Alert("Motion Tracker Disconnected!", AlertType.kError);
48+
49+
private final Alert lowBatteryAlert_ = new Alert("Quest battery is low!", AlertType.kWarning);
50+
51+
private final PoseEstimateConsumer estimateConsumer_;
52+
53+
public MotionTrackerVision(TrackerIO io, PoseEstimateConsumer estimateConsumer) {
54+
io_ = io;
55+
inputs_ = new TrackerInputsAutoLogged();
56+
estimateConsumer_ = estimateConsumer;
57+
}
58+
59+
@Override
60+
public void periodic() {
61+
io_.updateInputs(inputs_);
62+
Logger.processInputs("MotionTrackerVision", inputs_);
63+
64+
boolean disconnected = !inputs_.connected;
65+
disconnectedAlert_.set(disconnected);
66+
67+
lowBatteryAlert_.set(inputs_.batteryPercent < 20);
68+
69+
if (
70+
disconnected ||
71+
!inputs_.isTracking ||
72+
RobotState.isDisabled() ||
73+
!useQuestNav
74+
) return;
75+
76+
if (useQueuedFrames) {
77+
for (PoseFrame frame : inputs_.unreadFrames) {
78+
Pose2d robotPose = frame.questPose().transformBy(robotToQuest.inverse());
79+
double timestamp = frame.dataTimestamp();
80+
estimateConsumer_.integrate(robotPose, timestamp, stdDevs);
81+
}
82+
} else {
83+
PoseFrame frame = inputs_.unreadFrames[inputs_.unreadFrames.length - 1];
84+
estimateConsumer_.integrate(
85+
frame.questPose().transformBy(robotToQuest.inverse()),
86+
frame.dataTimestamp(),
87+
stdDevs
88+
);
89+
}
90+
}
91+
92+
public void setPose(Pose2d pose) {
93+
io_.setPose(pose.transformBy(robotToQuest));
94+
}
95+
96+
public boolean isConnected() {
97+
return inputs_.connected;
98+
}
99+
100+
}

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

100755100644
File mode changed.

0 commit comments

Comments
 (0)