diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4cd9eb7..80277c2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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( diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 9d47b89..11c7cc4 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -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; diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index 14f902d..5bd3706 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -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 diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 0250782..661b977 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -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 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.getOrangeTagDetected()){ + // if the detected tags match your alliances reef tags use their pose estimates + 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)); + } + + 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); 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(() -> { - drive( - -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband), - fieldRelative.getAsBoolean() - ); + 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); + }); + } } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 8b5ae11..96a5f04 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -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 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 gyroBuffer){ return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(), - getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose); + getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose); } public double getBlackTX(){