diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4df0d7e7..106b83cb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,7 +49,7 @@ public static final class FeatureFlags { public static final boolean runLocalizer = true; public static final boolean runIntake = true; - public static final boolean runScoring = false; + public static final boolean runScoring = true; public static final boolean runEndgame = false; public static final boolean runDrive = true; } @@ -477,6 +477,15 @@ public static HashMap getShooterMap() { // TODO: Find this return map; } + // Key - Velocity toward goal in meters per second + // Value - Shooter RPM to subtract + public static HashMap getShooterVelocityToRpmMap() { // TODO: Find this + HashMap map = new HashMap(); + map.put(0.0, 0.0); + map.put(10.0, 10.0); + return map; + } + // NOTE - This should be monotonically increasing // Key - Distance in meters // Value - Time in seconds diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index d74e8be1..f04d49de 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -18,6 +18,7 @@ import frc.robot.Constants.ScoringConstants; import frc.robot.utils.FieldFinder; import frc.robot.utils.FieldFinder.FieldLocations; +import frc.robot.utils.GeomUtil; import frc.robot.utils.InterpolateDouble; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -41,6 +42,7 @@ public class ScoringSubsystem extends SubsystemBase { private Supplier driveAllignedSupplier = () -> true; private final InterpolateDouble shooterInterpolated; + private final InterpolateDouble shooterVelocityToRpmInterpolated; private final InterpolateDouble aimerInterpolated; private final InterpolateDouble timeToPutAimDown; @@ -87,6 +89,8 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo, HoodIO hoodIo) { this.hoodIo = hoodIo; shooterInterpolated = new InterpolateDouble(ScoringConstants.getShooterMap()); + shooterVelocityToRpmInterpolated = + new InterpolateDouble(ScoringConstants.getShooterVelocityToRpmMap()); aimerInterpolated = new InterpolateDouble( @@ -105,6 +109,14 @@ private void idle() { shooterIo.setKickerVolts(0); hoodIo.setHoodAngleRad(0); + // TODO: Use actual data from findVelocityTowardPoint to adjust RPM, instead of just logging + Logger.recordOutput( + "scoring/velocityToGoal", + GeomUtil.findVelocityTowardPoint( + velocitySupplier.get(), + GeomUtil.poseToTranslation(poseSupplier.get()), + speakerSupplier.get())); + Logger.recordOutput("scoring/aimGoal", 0.0); if (!hasNote() && action == ScoringAction.INTAKE) { @@ -137,8 +149,16 @@ private void intake() { private void prime() { double distancetoGoal = findDistanceToGoal(); + double velocityToGoal = + GeomUtil.findVelocityTowardPoint( + velocitySupplier.get(), + GeomUtil.poseToTranslation(poseSupplier.get()), + speakerSupplier.get()); + Logger.recordOutput("scoring/aimGoal", aimerInterpolated.getValue(distancetoGoal)); - shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distancetoGoal)); + shooterIo.setShooterVelocityRPM( + shooterInterpolated.getValue(distancetoGoal) + - shooterVelocityToRpmInterpolated.getValue(velocityToGoal)); aimerIo.setAimAngleRad(aimerInterpolated.getValue(distancetoGoal), false); boolean shooterReady = diff --git a/src/main/java/frc/robot/utils/GeomUtil.java b/src/main/java/frc/robot/utils/GeomUtil.java index 59ff7525..4ab4085e 100644 --- a/src/main/java/frc/robot/utils/GeomUtil.java +++ b/src/main/java/frc/robot/utils/GeomUtil.java @@ -7,6 +7,7 @@ // license that can be found in the LICENSE file at // the root directory of this project. +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,6 +16,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.numbers.N2; /** Geometry utilities for working with translations, rotations, transforms, and poses. */ public class GeomUtil { @@ -80,6 +82,16 @@ public static Pose2d translationToPose(Translation2d translation) { return new Pose2d(translation, new Rotation2d()); } + /** + * Creates a translation from the x and y of a pose, ignoring rotation + * + * @param pose The pose to create the translation with + * @return The resulting translation + */ + public static Translation2d poseToTranslation(Pose2d pose) { + return new Translation2d(pose.getX(), pose.getY()); + } + /** * Creates a pure rotated pose * @@ -141,4 +153,39 @@ public static Translation2d translation3dTo2dXY(Translation3d translation) { public static Translation2d translation3dTo2dXZ(Translation3d translation) { return new Translation2d(translation.getX(), translation.getZ()); } + + /** + * Find the velocity of an object toward a target point + * + * @param velocity The velocity of the object + * @param currentTranslation The current position of the object + * @param targetTranslation The position of the target point + * @return The velocity toward the target point (a negative number if object is moving away) + */ + public static double findVelocityTowardPoint( + Vector velocity, + Translation2d currentTranslation, + Translation2d targetTranslation) { + // Calculate the velocity to the goal based on this formula + // https://web.ma.utexas.edu/users/m408m/Display12-3-4.shtml + + // Vector from current to target (translate target so that current is now 0, 0) + Translation2d currentToTarget = targetTranslation.minus(currentTranslation); + + // Get the angle of the vector from current to target + Rotation2d targetAngle = + Rotation2d.fromRadians(Math.atan2(currentToTarget.getY(), currentToTarget.getX())); + + // Get the angle of the velocity vector + Rotation2d velocityAngle = + Rotation2d.fromRadians(Math.atan2(velocity.get(1, 0), velocity.get(0, 0))); + + // Calculate theta, the angle between the speakerAngle and the current velocity angle + Rotation2d theta = velocityAngle.minus(targetAngle); + // Project the velocity vector onto the speaker vector with ||u|| * cos(theta) + // Where u is the velocity vector + double velocityToGoal = velocity.norm() * theta.getCos(); + + return velocityToGoal; + } }