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.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
driver.start().whileTrue(drivetrain.resetToVision());
/* /*
driver.rightBumper().whileTrue( driver.rightBumper().whileTrue(
drivetrain.goToPose( 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 kBackRightChassisAngularOffset = 0;
*/ */
public static final double kFrontLeftChassisAngularOffset = 0; public static final double kFrontLeftChassisAngularOffset = Math.PI;
public static final double kFrontRightChassisAngularOffset = Math.PI / 2; public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = -Math.PI / 2; public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
public static final double kBackRightChassisAngularOffset = Math.PI; public static final double kBackRightChassisAngularOffset = 0;
// 1, 7, 10 is the default for these three values // 1, 7, 10 is the default for these three values
public static final double kSysIDDrivingRampRate = 1; 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 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; 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 // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

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

@ -62,6 +62,8 @@ public class Drivetrain extends SubsystemBase {
public Vision vision; public Vision vision;
public Pose2d orangePose2d;
/** Creates a new DriveSubsystem. */ /** Creates a new DriveSubsystem. */
public Drivetrain() { public Drivetrain() {
m_frontLeft = new MAXSwerveModule( m_frontLeft = new MAXSwerveModule(
@ -141,6 +143,7 @@ public class Drivetrain extends SubsystemBase {
musicTimer.start(); musicTimer.start();
vision = new Vision(); vision = new Vision();
orangePose2d = new Pose2d();
} }
@Override @Override
@ -155,26 +158,34 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition() m_rearRight.getPosition()
}); });
gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp()); gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
// if the detected tags match your alliances reef tags use their pose estimates if(vision.getOrangeTagDetected()){
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){ // if the detected tags match your alliances reef tags use their pose estimates
Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer); if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
Logger.recordOutput("orange pose", new Pose3d(orangePose)); orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(orangePose, vision.getOrangeTimeStamp()); m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
}else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().equals(Alliance.Blue) && vision.getOrangeTagDetected()){ }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
Pose2d orangePose = vision.getOrangeGlobalPose(gyroBuffer); orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
Logger.recordOutput("orange pose", new Pose3d(orangePose)); m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
m_estimator.addVisionMeasurement(orangePose, 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));
}
Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
} }
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().equals(Alliance.Red) && vision.getOrangeTagDetected()){ if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer); Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
Logger.recordOutput("black pose", new Pose3d(blackPose)); Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); 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); Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
Logger.recordOutput("black pose", new Pose3d(blackPose)); Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
@ -182,7 +193,7 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition())); Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>20){ if(musicTimer.get()>10){
if (m_orchestra.isPlaying()) { if (m_orchestra.isPlaying()) {
m_orchestra.stop(); m_orchestra.stop();
} }
@ -232,12 +243,20 @@ public class Drivetrain extends SubsystemBase {
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot, public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
BooleanSupplier fieldRelative) { BooleanSupplier fieldRelative) {
return run(() -> { return run(() -> {
drive( if(DriverStation.getAlliance().get().equals(Alliance.Blue)){
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), drive(
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband), -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband), -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean() -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() { public Command zeroHeading() {
return run(() -> { return run(() -> {
gyro.reset(); gyro.reset();
m_estimator.resetRotation(new Rotation2d(0));
}); });
} }
@ -392,4 +412,10 @@ public class Drivetrain extends SubsystemBase {
public double getVelocity(){ public double getVelocity(){
return m_frontLeft.getState().speedMetersPerSecond; 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 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}; 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(){ 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){ 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], Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
VisionConstants.globalTagCoords[tagID][1], Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
new Rotation2d()); new Rotation2d());
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
@ -93,17 +93,17 @@ public class Vision{
.getTranslation(); .getTranslation();
Pose2d robotPose = new Pose2d( Pose2d robotPose = new Pose2d(
fieldToCameraTranslation, fieldToCameraTranslation,
new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[2]))) 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(camPose[3], camPose[4]), new Rotation2d(camPose[2])), Pose2d.kZero)); .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; return robotPose;
} }
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(), Timer.getTimestamp(), gyroBuffer, blackCamPose); getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
} }
public double getBlackTX(){ public double getBlackTX(){