Examples with TimestampedTranslation2d frc.robot.subsystems.vision.Vision.TimestampedTranslation2d used on opensource projects

Example 1 with TimestampedTranslation2d

use of frc.robot.subsystems.vision.Vision.TimestampedTranslation2d in project RobotCode2022 by Mechanical-Advantage.

the class Drive method addVisionMeasurement.

/**
 * Adds a new timestamped vision measurement
 */
public void addVisionMeasurement(TimestampedTranslation2d data) {
    Optional<Pose2d> historicalFieldToTarget = poseHistory.get(data.timestamp);
    if (historicalFieldToTarget.isPresent()) {
        // Get camera constants
        CameraPosition cameraPosition = VisionConstants.getCameraPosition(hoodStateSupplier.get());
        if (cameraPosition == null) {
            // Hood is moving, don't process frame
            return;
        }
        // Calculate new robot pose
        Rotation2d robotRotation = historicalFieldToTarget.get().getRotation();
        Rotation2d cameraRotation = robotRotation.rotateBy(cameraPosition.vehicleToCamera.getRotation());
        Transform2d fieldToTargetRotated = new Transform2d(FieldConstants.hubCenter, cameraRotation);
        Transform2d fieldToCamera = fieldToTargetRotated.plus(GeomUtil.transformFromTranslation(data.translation.unaryMinus()));
        Pose2d visionFieldToTarget = GeomUtil.transformToPose(fieldToCamera.plus(cameraPosition.vehicleToCamera.inverse()));
        if (visionFieldToTarget.getX() > FieldConstants.fieldLength || visionFieldToTarget.getX() < 0.0 || visionFieldToTarget.getY() > FieldConstants.fieldWidth || visionFieldToTarget.getY() < 0.0) {
            return;
        }
        // Save vision pose for logging
        noVisionTimer.reset();
        lastVisionPose = visionFieldToTarget;
        // Calculate vision percent
        double angularErrorScale = Math.abs(inputs.gyroYawVelocityRadPerSec) / visionMaxAngularVelocity;
        angularErrorScale = MathUtil.clamp(angularErrorScale, 0, 1);
        double visionShift = 1 - Math.pow(1 - visionShiftPerSec, 1 / visionNominalFramerate);
        visionShift *= 1 - angularErrorScale;
        // Reset pose
        Pose2d currentFieldToTarget = getPose();
        Translation2d fieldToVisionField = new Translation2d(visionFieldToTarget.getX() - historicalFieldToTarget.get().getX(), visionFieldToTarget.getY() - historicalFieldToTarget.get().getY());
        Pose2d visionLatencyCompFieldToTarget = new Pose2d(currentFieldToTarget.getX() + fieldToVisionField.getX(), currentFieldToTarget.getY() + fieldToVisionField.getY(), currentFieldToTarget.getRotation());
        if (resetOnVision) {
            setPose(new Pose2d(visionFieldToTarget.getX(), visionFieldToTarget.getY(), currentFieldToTarget.getRotation()), true);
            resetOnVision = false;
        } else {
            setPose(new Pose2d(currentFieldToTarget.getX() * (1 - visionShift) + visionLatencyCompFieldToTarget.getX() * visionShift, currentFieldToTarget.getY() * (1 - visionShift) + visionLatencyCompFieldToTarget.getY() * visionShift, currentFieldToTarget.getRotation()), false);
        }
    }
}