diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ea53a23..809dd0c 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -82,7 +82,7 @@ public static final class Drivetrainc { public static final double wheelDiameterMeters = Units.inchesToMeters(4.0); public static final double mu = 1; /* 70/83.2; */ // coefficient of friction. less means less max acceleration. - public static final double ROBOTMASS_KG = Units.lbsToKilograms(104+15);// TODO weigh actual robot with bumpers and battery later + public static final double ROBOTMASS_KG = Units.lbsToKilograms(128.1); // moment of inertia, kg/mm // USE ONSHAPE it has a calculator for this public static final double MOI = Math.pow(Units.inchesToMeters(1),2) * Units.lbsToKilograms(1) * 14040.21738; // 14040.21738 in^2 lb @@ -256,20 +256,17 @@ public static final class Autoc { } //#endregion public static class LimeLightc { - public static final String FRONT_LL = "FRONT_LL"; - public static final String BACK_LL = "BACK_LL"; + public static final String LEFT_LL = "LL_LEFT"; + public static final String RIGHT_LL = "LL_RIGHT"; public static final int[] LL_FRONT_SHOOTING_VALID_IDS = {9,10, 12, 7, 8,5, 11, 2, 18, 27, 21, 24, 25, 26}; - public static final int[] LL_BACK_SHOOTING_VALID_IDS = {13,14,15,16, 7, 12, 28, 23, 29, 30, 31, 32}; public static final int[] LL_GENERAL_VALID_IDS = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32}; public static final int[] LL_IDS_IGNORE_FOR_BLUE = {15,16,14,13,9,10,7,12}; public static final int[] LL_IDS_IGNORE_FOR_RED = {28, 23, 25, 26, 29,30,31,32}; public static final int[] LL_FRONT_GENERAL_CROP = {0,0,0,0}; //XMin, xMax, yMin, yMax all values (-1,1) - public static final int[] LL_BACK_GENERAL_CROP = {0,0,0,0}; //XMin, xMax, yMin, yMax all values (-1,1) public static final int[] LL_FRONT_SHOOTING_CROP = {0,0,0,0}; //XMin, xMax, yMin, yMax all values (-1,1) - public static final int[] LL_BACK_SHOOTING_CROP = {0,0,0,0}; //XMin, xMax, yMin, yMax all values (-1,1) } //#region Manipulator public static final class IntakeC { diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java index 0c602ae..83bd1e8 100644 --- a/src/main/java/org/carlmontrobotics/Robot.java +++ b/src/main/java/org/carlmontrobotics/Robot.java @@ -54,6 +54,7 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + m_robotContainer.drivetrain.resetPoseEstimator(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -75,6 +76,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.drivetrain.resetFieldOrientationWithAngle(m_robotContainer.drivetrain.getPoseEstimator().getEstimatedPosition().getRotation().getDegrees()); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 21ee8f1..cc6fe5b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -11,6 +11,7 @@ import org.carlmontrobotics.lib199.swerve.SwerveModule; import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; import org.carlmontrobotics.lib199.vendorLibs.Elastic; +import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers; import static org.carlmontrobotics.Config.CONFIG; @@ -417,11 +418,17 @@ public void setDrivingIdleMode(boolean brake) { private double timeStampLatestGyroWarning = 0; private Elastic.Notification notification = new Elastic.Notification(); + public void resetPoseEstimator() { + poseEstimator.resetPose(LimelightHelpers.getBotPose2d(RIGHT_LL)); + } + public void resetPoseEstimator(Pose2d pose) { + poseEstimator.resetPose(pose); + } + @Override public void periodic() { - detectCollision(); //This does nothing + //detectCollision(); //This does nothing PathPlannerLogging.logCurrentPose(getPose()); - double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { // module.turnPeriodic(); // module.turnPeriodic(); @@ -450,14 +457,9 @@ public void periodic() { } - // field.setRobotPose(odometry.getPoseMeters()); - - - - // odometry.update(gyro.getRotation2d(), getModulePositions()); - // poseEstimator.update(gyro.getRotation2d(), getModulePositions()); - + poseEstimator.update(gyro.getRotation2d(), getModulePositions()); + poseEstimator.addVisionMeasurement(LimelightHelpers.getBotPose2d(RIGHT_LL), Timer.getFPGATimestamp()); //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); // updateMT2PoseEstimator(); @@ -591,7 +593,7 @@ public void AutoBuilder() { //Supplier poseSupplier, this::getPose, // Robot pose supplier //Consumer resetPose, - this::setPoseWithLimelight, // Method to reset odometry (will be called if your auto has a starting pose) + this::resetPoseEstimator, // Method to reset odometry (will be called if your auto has a starting pose) //Supplier robotRelativeSpeedsSupplier, this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE //BiConsumer output, @@ -811,12 +813,6 @@ public void resetFieldOrientationBackwards() { public void resetFieldOrientationWithAngle(double angle) { fieldOffset = angle + gyro.getAngle(); } - public void resetPoseEstimator() { - // odometry.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); - - poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); - gyro.reset(); - } public SwerveDriveKinematics getKinematics() { return kinematics; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 4b420b2..d8f43f3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -3,6 +3,8 @@ import static org.carlmontrobotics.Constants.LimeLightc.*; +import java.util.random.RandomGenerator.LeapableGenerator; + import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers; import edu.wpi.first.math.MathUtil; @@ -18,8 +20,8 @@ public class Limelight extends SubsystemBase { // NEEDS TO SEE: Barge, Reef, Processor, Coral Dropoff public Limelight() { - LimelightHelpers.SetFiducialIDFiltersOverride(FRONT_LL, LL_GENERAL_VALID_IDS); - LimelightHelpers.SetFiducialIDFiltersOverride(BACK_LL, LL_GENERAL_VALID_IDS); + //LimelightHelpers.SetFiducialIDFiltersOverride(LEFT_LL, LL_GENERAL_VALID_IDS); + LimelightHelpers.SetFiducialIDFiltersOverride(RIGHT_LL, LL_GENERAL_VALID_IDS); } @@ -33,12 +35,12 @@ public void periodic() { */ public void setCrop(boolean shootingCrop) { if (shootingCrop) { - LimelightHelpers.setCropWindow(FRONT_LL, LL_FRONT_SHOOTING_CROP[0], LL_FRONT_SHOOTING_CROP[1], LL_FRONT_SHOOTING_CROP[2], LL_FRONT_SHOOTING_CROP[3]); - LimelightHelpers.setCropWindow(BACK_LL, LL_BACK_SHOOTING_CROP[0], LL_BACK_SHOOTING_CROP[1], LL_BACK_SHOOTING_CROP[2], LL_BACK_SHOOTING_CROP[3]); + LimelightHelpers.setCropWindow(RIGHT_LL, LL_FRONT_SHOOTING_CROP[0], LL_FRONT_SHOOTING_CROP[1], LL_FRONT_SHOOTING_CROP[2], LL_FRONT_SHOOTING_CROP[3]); + //LimelightHelpers.setCropWindow(LEFT_LL, LL_BACK_SHOOTING_CROP[0], LL_BACK_SHOOTING_CROP[1], LL_BACK_SHOOTING_CROP[2], LL_BACK_SHOOTING_CROP[3]); } else { - LimelightHelpers.setCropWindow(FRONT_LL, LL_FRONT_GENERAL_CROP[0], LL_FRONT_GENERAL_CROP[1], LL_FRONT_GENERAL_CROP[2], LL_FRONT_GENERAL_CROP[3]); - LimelightHelpers.setCropWindow(BACK_LL, LL_BACK_GENERAL_CROP[0], LL_BACK_GENERAL_CROP[1], LL_BACK_GENERAL_CROP[2], LL_BACK_GENERAL_CROP[3]); + LimelightHelpers.setCropWindow(RIGHT_LL, LL_FRONT_GENERAL_CROP[0], LL_FRONT_GENERAL_CROP[1], LL_FRONT_GENERAL_CROP[2], LL_FRONT_GENERAL_CROP[3]); + //LimelightHelpers.setCropWindow(BACK_LL, LL_BACK_GENERAL_CROP[0], LL_BACK_GENERAL_CROP[1], LL_BACK_GENERAL_CROP[2], LL_BACK_GENERAL_CROP[3]); } } @@ -51,27 +53,27 @@ public void setIDFilters(boolean redAlliance, boolean shootingFilter) { if (redAlliance) { if (shootingFilter) { int[] frontFilter = difference(LL_FRONT_SHOOTING_VALID_IDS, LL_IDS_IGNORE_FOR_RED); - int[] backFilter = difference(LL_BACK_SHOOTING_VALID_IDS, LL_IDS_IGNORE_FOR_RED); - LimelightHelpers.SetFiducialIDFiltersOverride(FRONT_LL, frontFilter); - LimelightHelpers.SetFiducialIDFiltersOverride(BACK_LL, backFilter); + //int[] backFilter = difference(LL_BACK_SHOOTING_VALID_IDS, LL_IDS_IGNORE_FOR_RED); + LimelightHelpers.SetFiducialIDFiltersOverride(RIGHT_LL, frontFilter); + //LimelightHelpers.SetFiducialIDFiltersOverride(LEFT_LL, backFilter); } else { int[] filter = difference(LL_GENERAL_VALID_IDS, LL_IDS_IGNORE_FOR_RED); - LimelightHelpers.SetFiducialIDFiltersOverride(FRONT_LL, filter); - LimelightHelpers.SetFiducialIDFiltersOverride(BACK_LL, filter); + LimelightHelpers.SetFiducialIDFiltersOverride(RIGHT_LL, filter); + //LimelightHelpers.SetFiducialIDFiltersOverride(LEFT_LL, filter); } } else { if (shootingFilter) { int[] frontFilter = difference(LL_FRONT_SHOOTING_VALID_IDS, LL_IDS_IGNORE_FOR_BLUE); - int[] backFilter = difference(LL_BACK_SHOOTING_VALID_IDS, LL_IDS_IGNORE_FOR_BLUE); - LimelightHelpers.SetFiducialIDFiltersOverride(FRONT_LL, frontFilter); - LimelightHelpers.SetFiducialIDFiltersOverride(BACK_LL, backFilter); + //int[] backFilter = difference(LL_BACK_SHOOTING_VALID_IDS, LL_IDS_IGNORE_FOR_BLUE); + LimelightHelpers.SetFiducialIDFiltersOverride(RIGHT_LL, frontFilter); + //LimelightHelpers.SetFiducialIDFiltersOverride(LEFT_LL, backFilter); } else { int[] filter = difference(LL_GENERAL_VALID_IDS, LL_IDS_IGNORE_FOR_BLUE); - LimelightHelpers.SetFiducialIDFiltersOverride(FRONT_LL, filter); - LimelightHelpers.SetFiducialIDFiltersOverride(BACK_LL, filter); + LimelightHelpers.SetFiducialIDFiltersOverride(RIGHT_LL, filter); + // LimelightHelpers.SetFiducialIDFiltersOverride(LEFT_LL, filter); } } }