diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c898e0b..a504e631 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc.robot.subsystems.Manipulator.IRSensorState; import frc.robot.subsystems.Manipulator.ManipulatorState; import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.drivetrain.PoseEstimator; import frc.robot.utils.GlobalDebuggable; /** @@ -50,6 +51,7 @@ public class RobotContainer { new Funnel()); private final LimeLight m_limeLight = new LimeLight(); + private final PoseEstimator m_poseEstimator = new PoseEstimator(m_drivetrain, m_limeLight); private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH); @@ -95,7 +97,7 @@ public RobotContainer() { * Use this method to define your trigger->command mappings. */ private void configureBindings() { - _driverController.cross().onTrue(_autoFactory.createfollow(m_limeLight.getCollectionTrajectory())); + _driverController.cross().onTrue(_autoFactory.createfollow(m_poseEstimator.getCollectionTrajectory())); _driverController.options().onTrue( new InstantCommand(() -> _fieldRelative = !_fieldRelative)); // toggle field diff --git a/src/main/java/frc/robot/constants/LimelightConstants.java b/src/main/java/frc/robot/constants/LimelightConstants.java index ec54f46f..86b6dc01 100644 --- a/src/main/java/frc/robot/constants/LimelightConstants.java +++ b/src/main/java/frc/robot/constants/LimelightConstants.java @@ -1,9 +1,6 @@ package frc.robot.constants; -import com.pathplanner.lib.PathPoint; - import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import frc.robot.motors.PIDFGains; /** @@ -29,14 +26,4 @@ public class LimelightConstants { public static final double pipeLineRetroReflective = 0; public static final double pipeLineAprilTags = 1; - - // TODO: these values are only approximation from PP - public static final Translation2d collectionTranslation = new Translation2d(14.0, 7.5); - public static final Rotation2d collectionHolonomicRotation = Rotation2d.fromDegrees(90); - public static final Rotation2d collectionHeading = Rotation2d.fromDegrees(90); - public static final PathPoint collectionPathPoint = new PathPoint( - collectionTranslation, - collectionHeading, - collectionHolonomicRotation); - } diff --git a/src/main/java/frc/robot/subsystems/LimeLight.java b/src/main/java/frc/robot/subsystems/LimeLight.java index e5226387..619846ab 100644 --- a/src/main/java/frc/robot/subsystems/LimeLight.java +++ b/src/main/java/frc/robot/subsystems/LimeLight.java @@ -4,11 +4,6 @@ package frc.robot.subsystems; -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPoint; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -19,7 +14,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.AutonomousConstants; import frc.robot.constants.LimelightConstants; public class LimeLight extends SubsystemBase { @@ -100,21 +94,4 @@ public BotPose getBotPose() { Timer.getFPGATimestamp() - (poseComponents[6] / 1000.0)); } - - public PathPlannerTrajectory getCollectionTrajectory() { - Pose2d botPose = getBotPose().getPose(); - if (botPose == null) { - return new PathPlannerTrajectory(); - } - - // heading straight to the collection using vector subtraction - Rotation2d startingHeading = LimelightConstants.collectionTranslation - .minus(botPose.getTranslation()).getAngle(); - - return PathPlanner.generatePath( - new PathConstraints(AutonomousConstants.kMaxSpeedMetersPerSecond, - AutonomousConstants.kMaxAccelerationMetersPerSecondSquared), - new PathPoint(botPose.getTranslation(), startingHeading, botPose.getRotation()), - LimelightConstants.collectionPathPoint); - } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index c3632149..a3bec589 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -164,6 +164,15 @@ public SwerveModulePosition[] getSwerveModulePositions() { return swerveModulePositions; } + public ChassisSpeeds getChassisSpeeds() { + SwerveModuleState[] states = new SwerveModuleState[this._modules.length]; + + for (int i = 0; i < states.length; i++) { + states[i] = this._modules[i].getState(); + } + return DrivetrainConstants.kinematics.toChassisSpeeds(states); + } + public void restartControllers() { vision_xController.reset(); vision_yController.reset(); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/PoseEstimator.java b/src/main/java/frc/robot/subsystems/drivetrain/PoseEstimator.java index b60f1884..a64543a0 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/PoseEstimator.java @@ -2,11 +2,17 @@ import java.util.Map; +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPoint; + import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; @@ -15,6 +21,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.AutonomousConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.subsystems.LimeLight; @@ -24,7 +31,16 @@ public class PoseEstimator extends SubsystemBase { private final Drivetrain drivetrain; private double lastVisionTimestamp = -1; - private final Pose2d robotAtSSS = new Pose2d(14.2, 7.6, Rotation2d.fromDegrees(90)); + + // TODO: these values are only approximation from PP + private static final Translation2d collectionTranslation = new Translation2d(14.0, 7.5); + private static final Rotation2d collectionHolonomicRotation = Rotation2d.fromDegrees(90); + private static final Rotation2d collectionHeading = Rotation2d.fromDegrees(90); + private static final PathPoint collectionPathPoint = new PathPoint( + collectionTranslation, + collectionHeading, + collectionHolonomicRotation); + private final double xTolerance = 0.2; // in meters private final double yTolerance = 0.1; // in meters private final double tTolerance = 10; // in degrees @@ -74,22 +90,22 @@ public PoseEstimator(Drivetrain drivetrain, LimeLight limeLight) { tab.addBoolean("X at SSS", this::xAtSSS).withPosition(0, 2).withSize(1, 1); tab.addNumber("X error", this::xError).withWidget(BuiltInWidgets.kNumberBar) .withProperties(Map.of( - "min", robotAtSSS.getX() - (xTolerance * 5), - "max", robotAtSSS.getX() + (xTolerance * 5))) + "min", collectionTranslation.getX() - (xTolerance * 5), + "max", collectionTranslation.getX() + (xTolerance * 5))) .withPosition(1, 2) .withSize(1, 1); tab.addBoolean("Y at SSS", this::yAtSSS).withPosition(0, 3).withSize(1, 1); tab.addNumber("Y error", this::yError).withWidget(BuiltInWidgets.kNumberBar) .withProperties(Map.of( - "min", robotAtSSS.getY() - (xTolerance * 5), - "max", robotAtSSS.getY() + (xTolerance * 5))) + "min", collectionTranslation.getY() - (xTolerance * 5), + "max", collectionTranslation.getY() + (xTolerance * 5))) .withPosition(1, 3) .withSize(1, 1); tab.addBoolean("T at SSS", this::tAtSSS).withPosition(0, 4).withSize(1, 1); tab.addNumber("T error", this::tError).withWidget(BuiltInWidgets.kNumberBar) .withProperties(Map.of( - "min", robotAtSSS.getRotation().getDegrees() - (tTolerance * 5), - "max", robotAtSSS.getRotation().getDegrees() + (tTolerance * 5))) + "min", collectionHolonomicRotation.getDegrees() - (tTolerance * 5), + "max", collectionHolonomicRotation.getDegrees() + (tTolerance * 5))) .withPosition(1, 4) .withSize(1, 1); tab.add("Field", field2d).withPosition(2, 0).withSize(6, 4); @@ -116,15 +132,15 @@ private boolean isVisionSynced() { } private double xError() { - return getCurrentPose().minus(robotAtSSS).getX(); + return getCurrentPose().getTranslation().minus(collectionTranslation).getX(); } private double yError() { - return getCurrentPose().minus(robotAtSSS).getY(); + return getCurrentPose().getTranslation().minus(collectionTranslation).getY(); } private double tError() { - return getCurrentPose().minus(robotAtSSS).getRotation().getDegrees(); + return getCurrentPose().getRotation().minus(collectionHolonomicRotation).getDegrees(); } private boolean xAtSSS() { @@ -174,4 +190,17 @@ public void resetFieldPosition() { setCurrentPose(new Pose2d()); } + public PathPlannerTrajectory getCollectionTrajectory() { + var pose = getCurrentPose(); + var speeds = drivetrain.getChassisSpeeds(); + + var velocity = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + var heading = Rotation2d.fromRadians(Math.atan2(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)); + return PathPlanner.generatePath( + new PathConstraints(AutonomousConstants.kMaxSpeedMetersPerSecond, + AutonomousConstants.kMaxAccelerationMetersPerSecondSquared), + new PathPoint(pose.getTranslation(), heading.plus(pose.getRotation()), pose.getRotation(), velocity), + collectionPathPoint); + } + } \ No newline at end of file