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

trying to fuse estimates

Browse Source
This commit is contained in:
Team 2648 2025-03-07 18:41:57 -05:00
parent 7c446fd874
commit 445ce9bf6f
5 changed files with 62 additions and 34 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

@ -152,6 +152,8 @@ public class RobotContainer {
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
driver.start().whileTrue(drivetrain.resetToVision());
/*
driver.rightBumper().whileTrue(
drivetrain.goToPose(

8
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -29,10 +29,10 @@ public class DrivetrainConstants {
public static final double kBackRightChassisAngularOffset = 0;
*/
public static final double kFrontLeftChassisAngularOffset = 0;
public static final double kFrontRightChassisAngularOffset = Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = -Math.PI / 2;
public static final double kBackRightChassisAngularOffset = Math.PI;
public static final double kFrontLeftChassisAngularOffset = Math.PI;
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
public static final double kBackRightChassisAngularOffset = 0;
// 1, 7, 10 is the default for these three values
public static final double kSysIDDrivingRampRate = 1;

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

@ -48,7 +48,7 @@ public class ModuleConstants {
public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
public static final InvertedValue kDriveInversionState = InvertedValue.CounterClockwise_Positive;
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

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

@ -62,6 +62,8 @@ public class Drivetrain extends SubsystemBase {
public Vision vision;
public Pose2d orangePose2d;
/** Creates a new DriveSubsystem. */
public Drivetrain() {
m_frontLeft = new MAXSwerveModule(
@ -141,6 +143,7 @@ public class Drivetrain extends SubsystemBase {
musicTimer.start();
vision = new Vision();
orangePose2d = new Pose2d();
}
@Override
@ -155,26 +158,34 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
});
gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp());
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
if(vision.getOrangeTagDetected()){
// 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()){
Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer);
Logger.recordOutput("orange pose", new Pose3d(orangePose));
m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp());
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer);
Logger.recordOutput("orange pose", new Pose3d(orangePose));
m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp());
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
}
Logger.recordOutput("orange pose", new Pose3d());
if(orangePose2d.equals(null)){
Logger.recordOutput("orange pose", new Pose3d());
}else{
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){
Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
}
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().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
}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());
@ -182,7 +193,7 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>20){
if(musicTimer.get()>10){
if (m_orchestra.isPlaying()) {
m_orchestra.stop();
}
@ -232,12 +243,20 @@ public class Drivetrain extends SubsystemBase {
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
BooleanSupplier fieldRelative) {
return run(() -> {
if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean()
);
}else{
drive(
MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean()
);}
});
}
@ -356,6 +375,7 @@ public class Drivetrain extends SubsystemBase {
public Command zeroHeading() {
return run(() -> {
gyro.reset();
m_estimator.resetRotation(new Rotation2d(0));
});
}
@ -392,4 +412,10 @@ public class Drivetrain extends SubsystemBase {
public double getVelocity(){
return m_frontLeft.getState().speedMetersPerSecond;
}
public Command resetToVision(){
return run(() -> {
m_estimator.resetPose(orangePose2d);
});
}
}

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

@ -33,7 +33,7 @@ public class Vision{
private DoubleSubscriber orangeFramerate;
private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 14.0-1.05, 7.308+2.75};
private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
public Vision(){
@ -78,14 +78,14 @@ public class Vision{
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(-camPose[1] - ty);
double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty));
Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(-tx)));
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(camPose[2])).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
new Rotation2d());
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
@ -93,17 +93,17 @@ public class Vision{
.getTranslation();
Pose2d robotPose = new Pose2d(
fieldToCameraTranslation,
new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[2])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(camPose[3], camPose[4]), new Rotation2d(camPose[2])), Pose2d.kZero));
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( camPose[2]))))
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(camPose[2]))), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
return robotPose;
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose);
getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
}
public double getBlackTX(){