Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

logging vision estimates

Browse Source
This commit is contained in:
Tylr-J42 2025-03-07 01:37:58 -05:00
parent 80b5908206
commit 2ae2beddfa
3 changed files with 56 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/java/frc/robot/RobotContainer.java
View File

@ -52,8 +52,6 @@ public class RobotContainer {
private SendableChooser<Command> autoChooser; private SendableChooser<Command> autoChooser;
private Vision vision;
private IntSupplier closestTag; private IntSupplier closestTag;
public RobotContainer() { public RobotContainer() {
@ -63,8 +61,6 @@ public class RobotContainer {
drivetrain = new Drivetrain(); drivetrain = new Drivetrain();
vision = new Vision();
elevator = new Elevator(); elevator = new Elevator();
//elevator = new ElevatorSysID(); //elevator = new ElevatorSysID();
@ -321,12 +317,12 @@ public class RobotContainer {
//sensorTab.add("odometry", drivetrain::getPose); //sensorTab.add("odometry", drivetrain::getPose);
apriltagTab.addDouble("Orange ID", vision::getOrangeClosestTag); apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag());
apriltagTab.addDouble("Orange tx", vision::getOrangeTX); apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX());
apriltagTab.addDouble("Orange ty", vision::getOrangeTY); apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY());
apriltagTab.addDouble("Orange dist", vision::getOrangeDist); apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
apriltagTab.addDouble("global x", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX()); apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
apriltagTab.addDouble("global y", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY()); apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag); // sensorTab.addDouble(" ID", vision::getOrangeClosestTag);

42
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -9,6 +9,8 @@ import java.util.Optional;
import java.util.function.BooleanSupplier; import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier; import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Orchestra; import com.ctre.phoenix6.Orchestra;
import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.AutoBuilder;
import com.studica.frc.AHRS; import com.studica.frc.AHRS;
@ -25,6 +27,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@ -33,6 +36,7 @@ import frc.robot.constants.AutoConstants;
import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants; import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants; import frc.robot.constants.VisionConstants;
import edu.wpi.first.math.geometry.Pose3d;
public class Drivetrain extends SubsystemBase { public class Drivetrain extends SubsystemBase {
// Create MAXSwerveModules // Create MAXSwerveModules
@ -56,6 +60,8 @@ public class Drivetrain extends SubsystemBase {
private PIDController pidTranslationX; private PIDController pidTranslationX;
private PIDController pidTranslationY; private PIDController pidTranslationY;
public Vision vision;
/** Creates a new DriveSubsystem. */ /** Creates a new DriveSubsystem. */
public Drivetrain() { public Drivetrain() {
m_frontLeft = new MAXSwerveModule( m_frontLeft = new MAXSwerveModule(
@ -133,6 +139,8 @@ public class Drivetrain extends SubsystemBase {
m_orchestra.play(); m_orchestra.play();
musicTimer.reset(); musicTimer.reset();
musicTimer.start(); musicTimer.start();
vision = new Vision();
} }
@Override @Override
@ -149,22 +157,30 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp()); gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp());
// if the detected tags match your alliances reef tags use their pose estimates // if the detected tags match your alliances reef tags use their pose estimates
/* if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){
if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); Logger.recordOutput("orange pose", new Pose3d(orangePose));
m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp());
}else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer);
Logger.recordOutput("orange pose", new Pose3d(orangePose));
m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp());
} }
if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
}else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
} }
*/
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>20){ if(musicTimer.get()>20){
if (m_orchestra.isPlaying()) { if (m_orchestra.isPlaying()) {
@ -369,10 +385,6 @@ public class Drivetrain extends SubsystemBase {
return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
} }
public void addVisionMeasurement(Pose2d pose, double timestamp){
m_estimator.addVisionMeasurement(pose, timestamp);
}
public double driveDistance(){ public double driveDistance(){
return m_frontLeft.getTotalDist(); return m_frontLeft.getTotalDist();
} }

48
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -1,14 +1,10 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.estimator.PoseEstimator;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTable;
@ -18,9 +14,9 @@ import frc.robot.constants.VisionConstants;
public class Vision{ public class Vision{
private DoubleSubscriber blackRobotRelativeX; private DoubleSubscriber black_tx;
private DoubleSubscriber blackRobotRelativeY; private DoubleSubscriber black_ty;
private DoubleSubscriber blackRobotRelativeZ; private DoubleSubscriber black_dist;
private DoubleSubscriber blackClosestTag; private DoubleSubscriber blackClosestTag;
private BooleanSubscriber blackTagDetected; private BooleanSubscriber blackTagDetected;
@ -36,8 +32,8 @@ public class Vision{
private DoubleSubscriber orangeFramerate; private DoubleSubscriber orangeFramerate;
private double[] orangeCamPose = {0,0,0,0,0}; private double[] orangeCamPose = {0.0, -5.0, -10, 14.0-7.673, 1.05-14.0, 7.308+2.75};
private double[] blackCamPose = {0,0,0,0,0}; private double[] blackCamPose = {0.0, -5.0, 10, 14.0-7.673, 1.05-14.0, 7.308+2.75};
public Vision(){ public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTableInstance inst = NetworkTableInstance.getDefault();
@ -45,9 +41,9 @@ public class Vision{
NetworkTable blackVisionTable = inst.getTable("black_Fiducial"); NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial"); NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0); black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0); black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0); black_dist = blackVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0); blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false); blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
@ -79,13 +75,13 @@ public class Vision{
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get())); return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
} }
public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){ public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
System.out.println(gyroBuffer.getSample(timestamp)); System.out.println(gyroBuffer.getSample(timestamp));
double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty); double distance2d = totalDist * Math.cos(-camPose[1] - ty);
Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx)); Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0] - tx));
Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0], Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1], VisionConstants.globalTagCoords[tagID][1],
@ -95,8 +91,8 @@ public class Vision{
.transformBy(new Transform2d(distance2d, 0.0, new Rotation2d())) .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
.getTranslation(); .getTranslation();
Pose2d robotPose = new Pose2d(fieldToCameraTranslation, Pose2d robotPose = new Pose2d(fieldToCameraTranslation,
new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0]))) new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(orangeCamPose[0])), Pose2d.kZero)); .transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(camPose[0])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get())); robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get()));
@ -105,19 +101,19 @@ public class Vision{
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){ public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(), return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer); getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose);
} }
public double getBlackRelativeX(){ public double getBlackTX(){
return blackRobotRelativeX.get(); return black_tx.get();
} }
public double getBlackRelativeY(){ public double getBlackTY(){
return blackRobotRelativeY.get(); return black_ty.get();
} }
public double getBlackRelativeZ(){ public double getBlackDist(){
return blackRobotRelativeZ.get(); return black_dist.get();
} }
public int getBlackClosestTag(){ public int getBlackClosestTag(){
@ -125,7 +121,7 @@ public class Vision{
} }
public double getBlackTimeStamp(){ public double getBlackTimeStamp(){
return blackRobotRelativeX.getLastChange(); return black_tx.getLastChange();
} }
public boolean getBlackTagDetected(){ public boolean getBlackTagDetected(){
@ -139,7 +135,7 @@ public class Vision{
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){ public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){ if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(), return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
); );
}else{ }else{
return new Pose2d(); return new Pose2d();