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

auto ends with correct gyro and apriltag tuning

Browse Source
This commit is contained in:
Team 2648 2025-03-13 23:58:46 -04:00
parent 00ecedf216
commit d85683377b
5 changed files with 30 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.OutlineViewer/outlineviewer.json
View File

@ -8,6 +8,12 @@
"NetworkTables Settings": {
"mode": "Client (NT4)"
},
"client@1": {
"Publishers": {
"open": true
},
"open": true
},
"client@2": {
"open": true
},

2
src/main/deploy/pathplanner/autos/fein.auto
View File

@ -7,7 +7,7 @@
{
"type": "path",
"data": {
"pathName": "fein"
"pathName": "New New Path"
}
}
]

20
src/main/deploy/pathplanner/paths/New New Path.path
View File

@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 7.0
"x": 2.5773565573770494,
"y": 6.506454918032786
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 7.0
"x": 3.5773565573770494,
"y": 6.506454918032786
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.891,
"y": 5.284
"x": 4.735143442622951,
"y": 6.506454918032786
},
"prevControl": {
"x": 3.891,
"y": 5.284
"x": 3.7351434426229506,
"y": 6.506454918032786
},
"nextControl": null,
"isLocked": false,
@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -119.99999999999999
"rotation": 90.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
"rotation": -90.0
},
"useDefaultConstraints": true
}

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

@ -179,7 +179,7 @@ public class Drivetrain extends SubsystemBase {
if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}
// if the detected tags match your alliances reef tags use their pose estimates
@ -197,7 +197,7 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
@ -208,8 +208,9 @@ public class Drivetrain extends SubsystemBase {
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
@ -256,7 +257,13 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_estimator.resetPose(
m_estimator.resetPosition(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()},
pose
);
}
@ -299,7 +306,7 @@ public class Drivetrain extends SubsystemBase {
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
Rotation2d.fromDegrees(getGyroValue()))
new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);

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

@ -106,7 +106,7 @@ public class Vision{
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
}