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": { "NetworkTables Settings": {
"mode": "Client (NT4)" "mode": "Client (NT4)"
}, },
"client@1": {
"Publishers": {
"open": true
},
"open": true
},
"client@2": { "client@2": {
"open": true "open": true
}, },

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

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

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

@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 2.0, "x": 2.5773565573770494,
"y": 7.0 "y": 6.506454918032786
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.0, "x": 3.5773565573770494,
"y": 7.0 "y": 6.506454918032786
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 4.891, "x": 4.735143442622951,
"y": 5.284 "y": 6.506454918032786
}, },
"prevControl": { "prevControl": {
"x": 3.891, "x": 3.7351434426229506,
"y": 5.284 "y": 6.506454918032786
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -119.99999999999999 "rotation": 90.0
}, },
"reversed": false, "reversed": false,
"folder": null, "folder": null,
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 0.0 "rotation": -90.0
}, },
"useDefaultConstraints": true "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.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){ 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 // 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.getBlackTagDetected()){
if(vision.getBlackDist() < 60){ 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()){ 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); blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 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())); 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. * @param pose The pose to which to set the odometry.
*/ */
public void resetOdometry(Pose2d pose) { 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 pose
); );
} }
@ -299,7 +306,7 @@ public class Drivetrain extends SubsystemBase {
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
Rotation2d.fromDegrees(getGyroValue())) new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds( SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); 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){ public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(), return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose); getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
} }