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

Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code

Browse Source
This commit is contained in:
Team 2648 2025-03-06 15:48:06 -05:00
commit aecc342dc4
3 changed files with 41 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -63,7 +63,7 @@ public class RobotContainer {
drivetrain = new Drivetrain();
vision = new Vision(drivetrain::getGyroValue);
vision = new Vision();
elevator = new Elevator();
//elevator = new ElevatorSysID();

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

@ -19,6 +19,7 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
@ -45,6 +46,8 @@ public class Drivetrain extends SubsystemBase {
// Odometry class for tracking robot pose
private SwerveDrivePoseEstimator m_estimator;
private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
public Orchestra m_orchestra = new Orchestra();
private Timer musicTimer = new Timer();
@ -144,6 +147,8 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
});
gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp());
// if the detected tags match your alliances reef tags use their pose estimates
/*
@ -351,6 +356,10 @@ public class Drivetrain extends SubsystemBase {
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
}
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
return gyroBuffer;
}
/**
* Returns the turn rate of the robot.
*

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

@ -2,10 +2,12 @@ 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.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable;
@ -32,9 +34,10 @@ public class Vision{
private DoubleSubscriber orangeFramerate;
private DoubleSupplier gyroAngle;
private double[] orangeCamPose = {0,0,0,0,0};
private double[] blackCamPose = {0,0,0,0,0};
public Vision(DoubleSupplier gyroAngle){
public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault();
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
@ -57,23 +60,45 @@ public class Vision{
orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
}
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
new Rotation2d());
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
}
public Pose2d getBlackGlobalPose(){
public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty);
Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx));
Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
new Rotation2d());
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
.transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
.getTranslation();
Pose2d robotPose = new Pose2d(fieldToCameraTranslation,
new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(orangeCamPose[0])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get()));
return robotPose;
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
}