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

Compare commits

base: Team_2648:dd50663b9ed247f07269d4d17efa31a1c05bd3b1
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup
..
compare: Team_2648:a089dddae32253fb9dee911c22fba095c9e73c63
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup

No commits in common. "dd50663b9ed247f07269d4d17efa31a1c05bd3b1" and "a089dddae32253fb9dee911c22fba095c9e73c63" have entirely different histories.
dd50663b9e ... a089dddae3

3 changed files with 30 additions and 36 deletions
Show Stats Expand all files Collapse all files

2
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -59,6 +59,6 @@ public class VisionConstants {
{4.993, 2.816, 5.272, 2.996} {4.993, 2.816, 5.272, 2.996}
}; };
public static final double latencyFudge = 0; public static final double latencyFudge = 0.080;
} }

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

@ -66,7 +66,6 @@ public class Drivetrain extends SubsystemBase {
public Vision vision; public Vision vision;
public Pose2d orangePose2d; public Pose2d orangePose2d;
public Pose2d blackPose2d;
/** Creates a new DriveSubsystem. */ /** Creates a new DriveSubsystem. */
public Drivetrain() { public Drivetrain() {
@ -95,7 +94,6 @@ public class Drivetrain extends SubsystemBase {
); );
gyro = new AHRS(NavXComType.kMXP_SPI); gyro = new AHRS(NavXComType.kMXP_SPI);
gyro.reset();
m_estimator = new SwerveDrivePoseEstimator( m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics, DrivetrainConstants.kDriveKinematics,
@ -106,11 +104,11 @@ public class Drivetrain extends SubsystemBase {
m_rearLeft.getPosition(), m_rearLeft.getPosition(),
m_rearRight.getPosition() m_rearRight.getPosition()
}, },
new Pose2d(), new Pose2d()
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
); );
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0); pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
pidHeading.enableContinuousInput(-180, 180); pidHeading.enableContinuousInput(-180, 180);
@ -154,7 +152,6 @@ public class Drivetrain extends SubsystemBase {
vision = new Vision(); vision = new Vision();
orangePose2d = new Pose2d(); orangePose2d = new Pose2d();
blackPose2d = new Pose2d();
} }
@Override @Override
@ -171,13 +168,12 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue()); gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360))); // if(vision.getOrangeDist() < 72){
// m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
//}
/*
if(vision.getOrangeTagDetected()){ if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
}
// 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().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){ if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer); orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
@ -188,24 +184,26 @@ public class Drivetrain extends SubsystemBase {
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp()); m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
} }
Logger.recordOutput("orange pose", new Pose3d()); Logger.recordOutput("orange pose", new Pose3d());
} if(orangePose2d.equals(null)){
Logger.recordOutput("orange pose", new Pose3d());
if(vision.getBlackTagDetected()){ }else{
if(vision.getBlackDist() < 60){ Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
} }
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
} }
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
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())); Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));

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

@ -15,8 +15,6 @@ import frc.robot.constants.VisionConstants;
public class Vision{ public class Vision{
private NetworkTable blackVisionTable;
private DoubleSubscriber black_tx; private DoubleSubscriber black_tx;
private DoubleSubscriber black_ty; private DoubleSubscriber black_ty;
private DoubleSubscriber black_dist; private DoubleSubscriber black_dist;
@ -26,8 +24,6 @@ public class Vision{
private DoubleSubscriber blackFramerate; private DoubleSubscriber blackFramerate;
private NetworkTable orangeVisionTable;
private DoubleSubscriber orange_tx; private DoubleSubscriber orange_tx;
private DoubleSubscriber orange_ty; private DoubleSubscriber orange_ty;
private DoubleSubscriber orange_dist; private DoubleSubscriber orange_dist;
@ -43,8 +39,8 @@ public class Vision{
public Vision(){ public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTableInstance inst = NetworkTableInstance.getDefault();
blackVisionTable = inst.getTable("black_Fiducial"); NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
orangeVisionTable = inst.getTable("orange_Fiducial"); NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0); black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0); black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
@ -107,7 +103,7 @@ 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(),
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose); getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
} }
public double getBlackTX(){ public double getBlackTX(){
@ -127,7 +123,7 @@ public class Vision{
} }
public double getBlackTimeStamp(){ public double getBlackTimeStamp(){
return black_tx.getLastChange()-VisionConstants.latencyFudge; return black_tx.getLastChange();
} }
public boolean getBlackTagDetected(){ public boolean getBlackTagDetected(){
@ -141,7 +137,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(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
); );
}else{ }else{
return new Pose2d(); return new Pose2d();