diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c00faaab..7b5773c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -179,7 +179,7 @@ private void configureDriverBindings(PatriBoxController controller) { .onTrue(intake.outCommand()); controller.leftBumper() - .toggleOnTrue(shooterCalc.prepareFireMovingCommand(() -> true, swerve::getPose)); + .toggleOnTrue(shooterCalc.prepareSWDSimCommand(() -> true, swerve::getPose, swerve::getRobotRelativeVelocity)); controller.leftTrigger() .onTrue(shooterCalc.resetShooter()); diff --git a/src/main/java/frc/robot/commands/NoteTrajectory.java b/src/main/java/frc/robot/commands/NoteTrajectory.java index 7c9e5776..940cc24d 100644 --- a/src/main/java/frc/robot/commands/NoteTrajectory.java +++ b/src/main/java/frc/robot/commands/NoteTrajectory.java @@ -18,41 +18,41 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; +import frc.robot.util.Constants; import frc.robot.util.Constants.NTConstants; public class NoteTrajectory extends Command { double x, x0, vx0, ax, z, z0, vz0, az, y, y0, vy0, ay; Supplier poseSupplier; - Supplier speedSupplier; - Supplier> pairSupplier; - + ChassisSpeeds initalSpeeds; Pose2d initialPose; - ChassisSpeeds speeds; - Pair speedAnglePair; + double initialVelocity; + double pivotAngle; + + Timer timer = new Timer(); - Pose3d currentNotePosition = new Pose3d(); /** * Represents a trajectory note. * - * @param poseSupplier a supplier for the pose of the trajectory - * @param speedSupplier a supplier for the chassis speeds of the trajectory - * @param pairSupplier a supplier for a pair of doubles representing additional information about the trajectory + * @param poseSupplier a supplier for the pose of the trajectory + * @param initialSpeeds a supplier for the chassis speeds of the trajectory + * @param initialVelocity the initial velocity of the note + * @param pivotAngle the angle of the pivot */ - public NoteTrajectory(Supplier poseSupplier, Supplier speedSupplier, Supplier> pairSupplier) { + public NoteTrajectory(Supplier poseSupplier, ChassisSpeeds initialSpeeds, double initialVelocity, double pivotAngle) { this.poseSupplier = poseSupplier; - this.speedSupplier = speedSupplier; - this.pairSupplier = pairSupplier; + this.initalSpeeds = initialSpeeds; + this.initialVelocity = initialVelocity; + this.pivotAngle = pivotAngle; } // Called when the command is initially scheduled. @Override public void initialize() { initialPose = poseSupplier.get(); - speeds = speedSupplier.get(); - speedAnglePair = pairSupplier.get(); timer.restart(); x = NTConstants.PIVOT_OFFSET_METERS.getX(); @@ -61,13 +61,14 @@ public void initialize() { x0 = NTConstants.PIVOT_OFFSET_METERS.getX(); y0 = 0; z0 = NTConstants.PIVOT_OFFSET_METERS.getY(); - vx0 = Rotation2d.fromDegrees(speedAnglePair.getSecond()).getCos() * speedAnglePair.getFirst() - + speeds.vxMetersPerSecond; - vy0 = speeds.vyMetersPerSecond; - vz0 = Rotation2d.fromDegrees(speedAnglePair.getSecond()).getSin() * speedAnglePair.getFirst(); + + vx0 = Rotation2d.fromDegrees(pivotAngle).getCos() * initialVelocity + initalSpeeds.vxMetersPerSecond; + vy0 = initalSpeeds.vyMetersPerSecond; + vz0 = Rotation2d.fromDegrees(pivotAngle).getSin() * initialVelocity; + ax = 0; ay = 0; - az = -9.8; + az = -Constants.GRAVITY; } // Called every time the scheduler runs while the command is scheduled. @@ -78,9 +79,9 @@ public void execute() { y = kinematicEquation3(y0, vy0, ay, t); z = kinematicEquation3(z0, vz0, az, t); - currentNotePosition = getNotePose(initialPose, speedAnglePair, x, y, z); + currentNotePosition = getNotePose(initialPose, pivotAngle, x, y, z); - RobotContainer.components3d[NTConstants.NOTE_INDEX] = getNotePose(initialPose, speedAnglePair, x, y, z).relativeTo(new Pose3d(poseSupplier.get())); + RobotContainer.components3d[NTConstants.NOTE_INDEX] = getNotePose(initialPose, pivotAngle, x, y, z).relativeTo(new Pose3d(poseSupplier.get())); } // Called once the command ends or is interrupted. @@ -100,14 +101,13 @@ public boolean isFinished() { * Calculates the pose of a note based on the given parameters. :) * * @param pose The initial pose of the robot. - * @param speedAnglePair A pair of values representing the speed and angle of - * the note. + * @param pivotAngle The angle of the pivot. * @param x The x-coordinate of the note. * @param y The y-coordinate of the note. * @param z The z-coordinate of the note. * @return The pose of the note. */ - Pose3d getNotePose(Pose2d pose, Pair speedAnglePair, double x, double y, double z) { + Pose3d getNotePose(Pose2d pose, double pivotAngle, double x, double y, double z) { return new Pose3d( new Translation3d(x, y, z).rotateBy(new Rotation3d( 0, @@ -122,7 +122,7 @@ Pose3d getNotePose(Pose2d pose, Pair speedAnglePair, double x, do 0), new Rotation3d( Units.degreesToRadians(90), - -Units.degreesToRadians(speedAnglePair.getSecond()), + -Units.degreesToRadians(pivotAngle), pose.getRotation().getRadians()))); } diff --git a/src/main/java/frc/robot/commands/PieceControl.java b/src/main/java/frc/robot/commands/PieceControl.java index e832a939..0e7c9682 100644 --- a/src/main/java/frc/robot/commands/PieceControl.java +++ b/src/main/java/frc/robot/commands/PieceControl.java @@ -56,7 +56,6 @@ public Command stopAllMotors() { return Commands.parallel( intake.stop(), indexer.stop(), - shooterCalc.stopMotors(), elevator.stop(), claw.stop()); } diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index fabd9048..450462c9 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -7,18 +7,18 @@ import edu.wpi.first.math.Pair; 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.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.DriverUI; import frc.robot.subsystems.shooter.*; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.ShooterConstants; import monologue.Logged; import monologue.Annotations.Log; +import frc.robot.util.Constants; import frc.robot.util.SpeedAngleTriplet; public class ShooterCalc implements Logged { @@ -35,7 +35,6 @@ public class ShooterCalc implements Logged { @Log.NT boolean atDesiredAngle = false , atDesiredRPM = false; - public ShooterCalc(Shooter shooter, Pivot pivot) { this.pivot = pivot; this.shooter = shooter; @@ -59,120 +58,11 @@ public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Supplier { SpeedAngleTriplet triplet = calculateSpeed(robotPose.get(), shootAtSpeaker.getAsBoolean()); - log(triplet); - pivot.setAngle(triplet.getAngle()); shooter.setSpeed(triplet.getSpeeds()); }, pivot, shooter); } - @Log.NT - Rotation2d currentAngleToSpeaker; - @Log.NT - Pose2d desiredSWDPose; - @Log.NT - double desiredMPSForNote = 0; - @Log.NT - double degreesToSpeakerReferenced = 0; - - - /** - * Calculates the angle to the speaker based on the robot's pose and velocity. - * This is to shoot while driving, but would also work while stationary. - * - * @param robotPose The current pose of the robot. - * @param robotVelocity The velocity of the robot. - * - * @return The angle to the speaker in the form of a Rotation2d object. - */ - public Rotation2d calculateSWDRobotAngleToSpeaker(Pose2d robotPose, ChassisSpeeds robotVelocity) { - double velocityTangentToSpeaker = getVelocityVectorToSpeaker(robotPose, robotVelocity).getX(); - - // Calculate the desired rotation to the speaker, taking into account the tangent velocity - Rotation2d desiredRotation2d = Rotation2d.fromRadians( - currentAngleToSpeaker.getRadians() - Math.atan2( - velocityTangentToSpeaker, - rpmToVelocity(calculateSpeed(robotPose, true).getSpeeds()) - ) - ); - - // Update the robot's pose with the desired rotation - desiredSWDPose = new Pose2d(robotPose.getTranslation(), desiredRotation2d); - - // Return the desired rotation - return desiredRotation2d; - } - - private Translation2d getVelocityVectorToSpeaker(Pose2d robotPose, ChassisSpeeds robotVelocity) { - // Calculate the robot's pose relative to the speaker - Pose2d poseRelativeToSpeaker = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); - - // Calculate the current angle to the speaker - currentAngleToSpeaker = new Rotation2d(poseRelativeToSpeaker.getX(), poseRelativeToSpeaker.getY()); - - // Convert the robot's velocity to a Rotation2d object - Rotation2d velocityRotation2d = new Rotation2d(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond); - - // Calculate the total speed of the robot - double totalSpeed = Math.hypot(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond); - - double angleDifference = velocityRotation2d.getRadians() - currentAngleToSpeaker.getRadians(); - // Calculate the component of the velocity that is tangent to the speaker - double velocityTangentToSpeaker = totalSpeed * Math.sin(angleDifference); - - double velocityNormalToSpeaker = totalSpeed * Math.cos(angleDifference); - - return new Translation2d(velocityTangentToSpeaker, velocityNormalToSpeaker); - } - - /** - * This method is averaging the speeds to make a rough estimate of the speed of the note (or the edge of the wheels). - * The formula used is V = 2π * D/2 * RPM/60. - * First, it converts from Rotations per Minute to Rotations per Second. - * Then, it converts from Rotations per Second to Radians per Second. - * Finally, it multiplies by the radius of the wheel contacting it. - * As v = rw (w = omega | angular velocity of wheel). - * - * Converts RPM (Revolutions Per Minute) to velocity in meters per second. - * @param speeds a pair of RPM values representing the speeds of two shooter wheels - * @return the velocity in meters per second - */ - public double rpmToVelocity(Pair speeds) { - double rotationsPerMinute = (speeds.getFirst() + speeds.getSecond()) / 2.0; - double rotationsPerSecond = rotationsPerMinute / 60.0; - double radiansPerSecond = rotationsPerSecond * Math.PI; - - double diameter = ShooterConstants.WHEEL_DIAMETER_METERS; - - desiredMPSForNote = diameter * radiansPerSecond; - - // Normally this is 1 radius * 2 pi - // but we are doing 2 radius * 1 pi - // because we are given a diameter - return diameter * radiansPerSecond; - } - - - /** - * Converts the velocity of the shooter wheel to RPM (Rotations Per Minute). - * Equation: ((V/(2π)) / (D/2)) * 60 = RPM - * - * @param noteVelocity the velocity of the shooter wheel in meters per second - * @return the RPM (Rotations Per Minute) of the shooter wheel - */ - public double velocityToRPM(double noteVelocity) { - double diameter = ShooterConstants.WHEEL_DIAMETER_METERS; - - // Convert velocity back to radians per second - double radiansPerSecond = noteVelocity / diameter; - - // Convert radians per second back to rotations per second - double rotationsPerSecond = radiansPerSecond / Math.PI; - - // Convert rotations per second back to rotations per minute - return rotationsPerSecond * 60.0; - } - /** * The function prepares a fire command by calculating the speed and angle for * the robot's shooter @@ -187,40 +77,44 @@ public double velocityToRPM(double noteVelocity) { * of the robot. It is of type `Supplier`. * @return The method is returning a Command object. */ - public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Supplier robotPose) { + public Command prepareSWDCommand(Supplier robotPose, Supplier speeds) { return Commands.run(() -> { - SpeedAngleTriplet triplet = calculateSpeed(robotPose.get(), shootAtSpeaker.getAsBoolean()); - - log(triplet); - - pivot.setAngle(triplet.getAngle()); - shooter.setSpeed(triplet.getSpeeds()); - }, pivot, shooter); + SpeedAngleTriplet triplet = calculateSWDTriplet(robotPose.get(), speeds.get()); + pivot.setAngle(triplet.getAngle()); + shooter.setSpeed(triplet.getSpeeds()); + }, pivot, shooter); } - public void logSpeeds(SpeedAngleTriplet triplet) { - desiredRSpeed = triplet.getRightSpeed(); - desiredLSpeed = triplet.getLeftSpeed(); - - realRSpeed = shooter.getSpeed().getSecond(); - realLSpeed = shooter.getSpeed().getFirst(); - } + public Command prepareSWDSimCommand(BooleanSupplier shootAtSpeaker, Supplier robotPose, Supplier robotSpeeds) { + return Commands.run(() -> { + Rotation2d angle = calculatePivotAngle(robotPose.get()); + Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), angle); - public void logAngles(SpeedAngleTriplet triplet) { - desiredAngle = -triplet.getAngle(); - realAngle = pivot.getAngle(); + pivot.setAngle(angle.getDegrees()); + shooter.setSpeed(speedsPair); + }, pivot, shooter); } - public void logAtDesired() { - atDesiredAngle = pivotAtDesiredAngle(); - atDesiredRPM = shooterAtDesiredRPM(); + /** + * Calculates the pivot angle based on the robot's pose. + * + * @param robotPose The pose of the robot. + * @return The calculated pivot angle. + */ + public Rotation2d calculatePivotAngle(Pose2d robotPose) { + // Add the pivot offset to the robot's pose + robotPose = robotPose.plus(new Transform2d(0.112, 0, robotPose.getRotation())); + // Calculate the robot's pose relative to the speaker's position + robotPose = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); + + // Calculate the distance in feet from the robot to the speaker + double distanceMeters = robotPose.getTranslation().getNorm(); + + // Return a new rotation object that represents the pivot angle + // The pivot angle is calculated based on the speaker's height and the distance to the speaker + return new Rotation2d(distanceMeters, 3); } - public void log(SpeedAngleTriplet triplet) { - logSpeeds(triplet); - logAngles(triplet); - logAtDesired(); - } /** * Determines if the pivot rotation is at its target with a small * tolerance @@ -235,7 +129,6 @@ public BooleanSupplier atDesiredAngle() { ShooterConstants.PIVOT_DEADBAND) == 0); } - // TODO: Implement a way to get the RPM of the shooter /** * The function is a BooleanSupplier that represents the the condition of * the velocity of the motor being equal to its targetVelocity @@ -287,10 +180,6 @@ public Command resetShooter() { // Gets a SpeedAngleTriplet by interpolating values from a map of already // known required speeds and angles for certain poses public SpeedAngleTriplet calculateSpeed(Pose2d robotPose, boolean shootingAtSpeaker) { - // Constants have blue alliance positions at index 0 - // and red alliance positions at index 1 - int positionIndex = FieldConstants.IS_BLUE_ALLIANCE() ? 0 : 1; - // Get our position relative to the desired field element if (shootingAtSpeaker) { robotPose = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); @@ -324,88 +213,172 @@ public boolean shooterAtDesiredRPM() { return atDesiredRPM().getAsBoolean(); } + + @Log.NT + Rotation2d currentAngleToSpeaker; + @Log.NT + Pose2d desiredSWDPose; + @Log.NT + double desiredMPSForNote = 0; + @Log.NT + double degreesToSpeakerReferenced = 0; + /** - * Stops the motors of the shooter and pivot subsystems. + * Calculates the angle to the speaker based on the robot's pose and velocity. + * This is to shoot while driving, but would also work while stationary. * - * @return The command to stop the motors. + * @param robotPose The current pose of the robot. + * @param robotVelocity The velocity of the robot. + * + * @return The angle to the speaker in the form of a Rotation2d object. */ - public Command stopMotors() { - return Commands.parallel( - shooter.stop(), - pivot.stop()); - } + public Rotation2d calculateSWDRobotAngleToSpeaker(Pose2d robotPose, ChassisSpeeds robotVelocity) { + Translation2d velocityVectorToSpeaker = getVelocityVectorToSpeaker(robotPose, robotVelocity); + double velocityTangent = velocityVectorToSpeaker.getX(); - public Command getNoteTrajectoryCommand(Supplier pose, Supplier speeds) { - SpeedAngleTriplet calculationTriplet = calculateSWDSpeedAngleTripletToSpeaker(pose, speeds); - - return Commands.runOnce(() -> new NoteTrajectory( - pose, - speeds, - () -> Pair.of( - rpmToVelocity(calculationTriplet.getSpeeds()), - calculationTriplet.getAngle()) - ).schedule()); + Pose2d poseRelativeToSpeaker = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); + Rotation2d currentAngleToSpeaker = new Rotation2d(poseRelativeToSpeaker.getX(), poseRelativeToSpeaker.getY()); + double velocityArcTan = Math.atan2( + velocityTangent, + rpmToVelocity(calculateSpeed(robotPose, true).getSpeeds()) + ); + // Calculate the desired rotation to the speaker, taking into account the tangent velocity + // Add PI because the speaker opening is the opposite direction that the robot needs to be facing + Rotation2d desiredRotation2d = Rotation2d.fromRadians( + currentAngleToSpeaker.getRadians() + velocityArcTan + Math.PI + ); + + // Update the robot's pose with the desired rotation + desiredSWDPose = new Pose2d(robotPose.getTranslation(), desiredRotation2d); + + // Return the desired rotation + return desiredRotation2d; } - - private SpeedAngleTriplet calculateSWDSpeedAngleTripletToSpeaker(Supplier pose, Supplier speeds){ - double dt = DriverUI.currentTimestamp - DriverUI.previousTimestamp; - return new SpeedAngleTriplet( - calculateSWDShooterSpeedsToSpeaker(pose, speeds, dt), - calculateSWDPivotAngleToSpeaker(pose, speeds, dt).getDegrees() - ); + private Translation2d getVelocityVectorToSpeaker(Pose2d robotPose, ChassisSpeeds robotVelocity) { + // Calculate the robot's pose relative to the speaker + Pose2d poseRelativeToSpeaker = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); + + // Calculate the current angle to the speaker + currentAngleToSpeaker = new Rotation2d(poseRelativeToSpeaker.getX(), poseRelativeToSpeaker.getY()); + + // Convert the robot's velocity to a Rotation2d object + Rotation2d velocityRotation2d = new Rotation2d(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond); + + // Calculate the total speed of the robot + double totalSpeed = Math.hypot(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond); + + double angleDifference = velocityRotation2d.getRadians() - currentAngleToSpeaker.getRadians(); + // Calculate the component of the velocity that is tangent to the speaker + double velocityTangentToSpeaker = totalSpeed * Math.sin(angleDifference); + + double velocityNormalToSpeaker = totalSpeed * Math.cos(angleDifference); + + return new Translation2d(velocityTangentToSpeaker, velocityNormalToSpeaker); } /** - * Calculates the shooter speeds required to reach the speaker position. + * This method is averaging the speeds to make a rough estimate of the speed of the note (or the edge of the wheels). + * The formula used is V = 2π * D/2 * RPM/60. + * First, it converts from Rotations per Minute to Rotations per Second. + * Then, it converts from Rotations per Second to Radians per Second. + * Finally, it multiplies by the radius of the wheel contacting it. + * As v = rw (w = omega | angular velocity of wheel). * - * @param pose a supplier of the robot's current pose - * @param speeds a supplier of the robot's current chassis speeds - * @param dt the time interval for integration - * @return a pair of shooter speeds (left and right) required to reach the speaker position + * Converts RPM (Revolutions Per Minute) to velocity in meters per second. + * @param speeds a pair of RPM values representing the speeds of two shooter wheels + * @return the velocity in meters per second */ - private Pair calculateSWDShooterSpeedsToSpeaker(Supplier pose, Supplier speeds, double dt) { - Pose2d estimatedPose = integratePosition(pose, speeds, dt); - - SpeedAngleTriplet futureTriplet = - ShooterConstants.INTERPOLATION_MAP.get( - estimatedPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()).getX() - ); - - - double velocityNormalToSpeaker = getVelocityVectorToSpeaker(pose.get(), speeds.get()).getY(); - double averageAddedRPM = velocityToRPM(velocityNormalToSpeaker); + public double rpmToVelocity(Pair speeds) { + double rotationsPerMinute = (speeds.getFirst() + speeds.getSecond()) / 2.0; + double rotationsPerSecond = rotationsPerMinute / 60.0; + double radiansPerSecond = rotationsPerSecond * Math.PI; - double averageTotalRPM = futureTriplet.getAverageSpeed() - averageAddedRPM; + double diameter = ShooterConstants.WHEEL_DIAMETER_METERS; - // Un-average the rpm to get the shooter wheels up to speed - // Use the difference at the initial guess to get a gooder average - double difference = futureTriplet.getLeftSpeed() - futureTriplet.getRightSpeed(); + desiredMPSForNote = diameter * radiansPerSecond; - return Pair.of( - averageTotalRPM + difference, - averageTotalRPM - difference - ); + // Normally this is 1 radius * 2 pi + // but we are doing 2 radius * 1 pi + // because we are given a diameter + return diameter * radiansPerSecond; } + /** + * Converts the velocity of the note to RPM (Rotations Per Minute). + * Equation: ((V/(2π)) / (D/2)) * 60 = RPM + * + * @param noteVelocity the velocity of the initial note in meters per second + * @return the RPM (Rotations Per Minute) of the shooter wheel + */ + public double velocityToRPM(double noteVelocity) { + double diameter = ShooterConstants.WHEEL_DIAMETER_METERS; + + // Convert velocity back to radians per second + double radiansPerSecond = noteVelocity / (2*Math.PI); + + // Convert radians per second back to rotations per second + double rotationsPerSecond = radiansPerSecond / (diameter/2); + + // Convert rotations per second back to rotations per minute + return rotationsPerSecond * 60.0; + } - private Rotation2d calculateSWDPivotAngleToSpeaker(Supplier pose, Supplier speeds, double dt) { - Pose2d estimatedPose = integratePosition(pose, speeds, dt); - - estimatedPose = estimatedPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); - return Rotation2d.fromDegrees(ShooterConstants.INTERPOLATION_MAP.get(estimatedPose.getX()).getAngle()); + /** + * Calculates the shooter speeds required to reach the speaker position. + * + * @param robotPose the current pose of the robot + * @param robotSpeeds the current chassis speeds of the robot + * @return a pair of shooter speeds (left and right) required to reach the speaker position + */ + private Pair calculateShooterSpeedsForApex(Pose2d robotPose, Rotation2d pivotAngle) { + double desiredRPM = velocityToRPM(Math.sqrt(2 * Constants.GRAVITY * FieldConstants.SPEAKER_HEIGHT) / (pivotAngle.getSin())); + return Pair.of(desiredRPM, desiredRPM); } - private Pose2d integratePosition(Supplier pose, Supplier speeds, double dt) { - Pose2d robotPose = pose.get(); - ChassisSpeeds robotSpeeds = speeds.get(); - - Pose2d estimatedPose = robotPose.exp(new Twist2d( - robotSpeeds.vxMetersPerSecond * dt, - robotSpeeds.vyMetersPerSecond * dt, - robotSpeeds.omegaRadiansPerSecond * dt) + public Command getNoteTrajectoryCommand(Supplier pose, Supplier speeds) { + return Commands.runOnce( + () -> { + SpeedAngleTriplet calculationTriplet = calculateSWDTriplet(pose.get(), speeds.get()); + + new NoteTrajectory( + pose, + speeds.get(), + rpmToVelocity(calculationTriplet.getSpeeds()), + calculationTriplet.getAngle() + ).schedule(); + } ); - return estimatedPose; + } + /** + * Calculates the shooter speeds required to reach the speaker position. + * + * @param pose a supplier of the robot's current pose + * @param speeds a supplier of the robot's current chassis speeds + * @param dt the time interval for integration + * @return a pair of shooter speeds (left and right) required to reach the speaker position + */ + private SpeedAngleTriplet calculateSWDTriplet(Pose2d pose, ChassisSpeeds speeds) { + Pose2d currentPose = pose; + Rotation2d pivotAngle = calculatePivotAngle(currentPose); + SpeedAngleTriplet currentTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(currentPose, pivotAngle), pivotAngle.getDegrees()); + double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds).getY(); + + double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); + double v0z = Math.sqrt(Constants.GRAVITY*2*2.08); + double v0x = originalv0 * Math.cos(Units.degreesToRadians(currentTriplet.getAngle())) + normalVelocity; + + double newv0 = Math.hypot(v0x, v0z); + Rotation2d newAngle = new Rotation2d(v0x, v0z); + + return + SpeedAngleTriplet.of( + Pair.of( + velocityToRPM(newv0), + velocityToRPM(newv0) + ), + newAngle.getDegrees() + ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 8a5b09e7..0af2aceb 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -502,11 +502,13 @@ public static enum GameMode { // All points are in meters and radians // All relative to the blue origin // Blue Speaker - new Pose2d(0, 5.547, Rotation2d.fromDegrees(180)), + new Pose2d(0, 5.547, Rotation2d.fromDegrees(0)), // Red Speaker - new Pose2d(FIELD_WIDTH_METERS, 5.547, Rotation2d.fromDegrees(180)), + new Pose2d(FIELD_WIDTH_METERS, 5.547, Rotation2d.fromDegrees(0)), }; + public static final double SPEAKER_HEIGHT = 2.08; + private static final Pose2d[] AMP_POSITIONS = new Pose2d[] { // All points are in meters and radians // All relative to the blue origin @@ -543,4 +545,6 @@ public static final class NTConstants { public static final Translation2d PIVOT_OFFSET_METERS = new Translation2d(0.112, 0.21); } + public static final double GRAVITY = 9.8; + } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/SpeedAngleTriplet.java b/src/main/java/frc/robot/util/SpeedAngleTriplet.java index 455b8f91..770b346f 100644 --- a/src/main/java/frc/robot/util/SpeedAngleTriplet.java +++ b/src/main/java/frc/robot/util/SpeedAngleTriplet.java @@ -67,6 +67,10 @@ public static SpeedAngleTriplet of(Double leftSpeed, Double rightSpeed, Double a return new SpeedAngleTriplet(leftSpeed, rightSpeed, angle); } + public static SpeedAngleTriplet of(Pair speeds, Double angle) { + return new SpeedAngleTriplet(speeds, angle); + } + /** * Interpolates between this SpeedAngleTriplet and another SpeedAngleTriplet * based on a given parameter.