From 9b3c275c7012efe415af441c5566ed7ad3687f37 Mon Sep 17 00:00:00 2001 From: mixxlto Date: Thu, 29 Jan 2026 14:40:38 -0800 Subject: [PATCH] so mega lucario mightttt work --- .../robot/commands/gpm/TurretAutoShoot.java | 164 ++++++++---------- .../subsystems/turret/ShotInterpolation.java | 13 ++ .../subsystems/turret/TurretConstants.java | 3 + 3 files changed, 86 insertions(+), 94 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java index 2949fd1..52ecebf 100644 --- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java @@ -1,139 +1,115 @@ package frc.robot.commands.gpm; -import java.lang.reflect.Field; - import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Robot; +import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.turret.ShotInterpolation; import frc.robot.subsystems.turret.Turret; +import frc.robot.subsystems.turret.TurretConstants; import frc.robot.util.FieldZone; -import frc.robot.util.ShootingTarget; -import frc.robot.util.Vision.TurretVision; public class TurretAutoShoot extends Command { private Turret turret; private Drivetrain drivetrain; - private TurretVision turretVision; - private double fieldAngleRad; private double turretSetpoint; - private double adjustedSetpoint; - private double yawToTagCamera; - private double yawToTag; - - private boolean turretVisionEnabled = false; - private boolean SOTM = true; - private Translation2d drivepose; - public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision) { + + private Pose2d drivepose; + + private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); + private Rotation2d lastTurretAngle; + private Rotation2d turretAngle; + private double turretVelocity; + private final double phaseDelay = 0.03; + + public TurretAutoShoot(Turret turret, Drivetrain drivetrain) { this.turret = turret; this.drivetrain = drivetrain; - this.turretVision = turretVision; - drivepose = drivetrain.getPose().getTranslation(); + drivepose = drivetrain.getPose(); addRequirements(turret); } - public TurretAutoShoot(Turret turret, Drivetrain drivetrain) { - this(turret, drivetrain, null); - } - - public void updateTurretSetpoint(Translation2d drivepose) { + public void updateTurretSetpoint(Pose2d drivepose) { - FieldZone currentZone = getZone(drivepose); - Translation2d target; - switch (currentZone) { - case NEUTRAL: - target = FieldConstants.getAllianceTranslation(leftSide(drivepose)).toTranslation2d(); - case OPPOSITION: - target = FieldConstants.getAllianceTranslation(leftSide(drivepose)).toTranslation2d(); - case ALLIANCE: - target = FieldConstants.getAllianceTranslation(leftSide(drivepose)).toTranslation2d(); // For the shooter we will want to check if active but turret should be fine - default: - target = FieldConstants.getHubTranslation().toTranslation2d(); - - // I also made this for if we want to shoot to the opposing teams area (though we would never haha): - // target = FieldConstants.getOppositionTranslation(leftSide(drivepose)).toTranslation2d(); - } - - double D_y; - double D_x; - // TODO: Change time to goal on actual comp bot - double timeToGoal = 0.67; + Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); + Pose2d turretPosition = drivepose.transformBy(new Transform2d((TurretConstants.DISTANCE_FROM_ROBOT_CENTER), new Rotation2d())); + double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); // If the robot is moving, adjust the target position based on velocity - if (SOTM) { - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); - double xVel = fieldRelVel.vxMetersPerSecond; - double yVel = fieldRelVel.vyMetersPerSecond; - - D_y = target.getY() - drivepose.getY() - timeToGoal * yVel; - D_x = target.getX() - drivepose.getX() - timeToGoal * xVel; - } else { - D_y = target.getY() - drivepose.getY(); - D_x = target.getX() - drivepose.getX(); + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); + + double turretVelocityX = + fieldRelVel.vxMetersPerSecond + + fieldRelVel.omegaRadiansPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); + double turretVelocityY = + fieldRelVel.vyMetersPerSecond + + fieldRelVel.omegaRadiansPerSecond + * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) + - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); + + // Account for imparted velocity by robot (turret) to offset + double timeOfFlight; + Pose2d lookaheadPose = turretPosition; + double lookaheadTurretToTargetDistance = turretToTargetDistance; + for (int i = 0; i < 20; i++) { + timeOfFlight = ShotInterpolation.timeOfFlightMap.get(lookaheadTurretToTargetDistance); + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + lookaheadPose = + new Pose2d( + turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), + turretPosition.getRotation()); + lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } - - // Calculate the field-relative angle - fieldAngleRad = Math.atan2(D_y, D_x); - - // Calculate robot heading and adjust for reverse drive - double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment - - // Calculate turret setpoint (angle relative to robot heading) - turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0); - - // System.out.println("Aligning the turn to degree angle: " + turretSetpoint); - } - - public void adjustWithTurretCam() { - if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){ - // Adjust turret setpoint based on vision input - if(Robot.getAlliance() == Alliance.Blue){ - yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get()); - } - else{ - yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get()); - } - double error = yawToTagCamera - yawToTag; - adjustedSetpoint = turretSetpoint + error; + turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + if (lastTurretAngle == null) { + lastTurretAngle = turretAngle; } + turretVelocity = + turretAngleFilter.calculate( + turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); + lastTurretAngle = turretAngle; + + // Add 180 since drivetrain is backwards + turretSetpoint = MathUtil.inputModulus(turretAngle.getDegrees() + 180, -180.0, 180.0); } - public void updateYawToTag(){ - // Calculate the yaw offset to the tag - double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY(); - double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX(); - double angleToTag = Units.radiansToDegrees(Math.atan(D_y / D_x)); - yawToTag = angleToTag - Units.radiansToDegrees(fieldAngleRad); + public void updateDrivePose(){ + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + drivepose = drivetrain.getPose().exp( + new Twist2d( + robotRelVel.vxMetersPerSecond * phaseDelay, + robotRelVel.vyMetersPerSecond * phaseDelay, + robotRelVel.omegaRadiansPerSecond * phaseDelay)); } @Override public void initialize() { - // Initialize setpoint calculation and set the initial goal for the turret + updateDrivePose(); updateTurretSetpoint(drivepose); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2)); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity); } @Override public void execute() { - // Continuously update setpoints and adjust based on vision if available + updateDrivePose(); updateTurretSetpoint(drivepose); - updateYawToTag(); - if(turretVision != null && turretVisionEnabled && turret.atGoal()){ - adjustWithTurretCam(); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(adjustedSetpoint)), -drivetrain.getAngularRate(2)); - } else{ - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) * 1.0); - } + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity); } @Override diff --git a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java new file mode 100644 index 0000000..8aaf253 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.turret; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +public class ShotInterpolation { + public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); + public static Object timeOfFlight; + + static{ + timeOfFlightMap.put(0.0, 0.67); + timeOfFlightMap.put(0.0, 0.67); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 18de26b..60c8753 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.turret; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; public class TurretConstants { @@ -13,4 +14,6 @@ public class TurretConstants { public static double TURRET_RADIUS = TURRET_WIDTH / 2; public static double ROTATIONAL_VELOCITY_CONSTANT = 0.2; + + public static Translation2d DISTANCE_FROM_ROBOT_CENTER = new Translation2d(0, 0); } -- 2.39.5