From 3531a58ea82205dbb4b4e9d234ce990452cc1f3f Mon Sep 17 00:00:00 2001 From: Jacob Hotz <77470805+Jacob1010-h@users.noreply.github.com> Date: Tue, 6 Feb 2024 23:36:16 -0800 Subject: [PATCH 1/9] Some bugs :// Co-authored-by: Alexander Hamilton --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/ShooterCalc.java | 51 ++++++++++++++++--- 2 files changed, 44 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c00faaab..0582902a 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.prepareFireViaMathCommand(() -> true, swerve::getPose, swerve::getRobotRelativeVelocity)); controller.leftTrigger() .onTrue(shooterCalc.resetShooter()); diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index fabd9048..f925acc9 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -198,6 +198,27 @@ public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Supplier }, pivot, shooter); } + public Command prepareFireViaMathCommand(BooleanSupplier shootAtSpeaker, Supplier robotPose, Supplier speeds) { + return Commands.run(() -> { + Rotation2d angle = calculateSWDPivotAngleToSpeaker(robotPose.get(), speeds.get(), 0.02); + Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), speeds.get(), 0.02); + + pivot.setAngle(angle.getDegrees()); + shooter.setSpeed(speedsPair); + }, pivot, shooter); + } + + public Command prepareFireMovingCommand(Supplier robotPose, Supplier speeds) { + return Commands.run(() -> { + SpeedAngleTriplet triplet = calculateSWDSpeedAngleTripletToSpeaker(robotPose, speeds); + + log(triplet); + + pivot.setAngle(triplet.getAngle()); + shooter.setSpeed(triplet.getSpeeds()); + }, pivot, shooter); + } + public void logSpeeds(SpeedAngleTriplet triplet) { desiredRSpeed = triplet.getRightSpeed(); desiredLSpeed = triplet.getLeftSpeed(); @@ -335,6 +356,20 @@ public Command stopMotors() { pivot.stop()); } + /** + * 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, ChassisSpeeds robotSpeeds, double dt) { + Rotation2d pivotAngle = calculateSWDPivotAngleToSpeaker(robotPose, robotSpeeds, dt); + double d = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()).getTranslation().getNorm(); + double desiredRPM = velocityToRPM(Math.sqrt(19.6*d)/(pivotAngle.getSin())); + return Pair.of(desiredRPM, desiredRPM); + } + public Command getNoteTrajectoryCommand(Supplier pose, Supplier speeds) { SpeedAngleTriplet calculationTriplet = calculateSWDSpeedAngleTripletToSpeaker(pose, speeds); @@ -351,8 +386,8 @@ private SpeedAngleTriplet calculateSWDSpeedAngleTripletToSpeaker(Supplier calculateSWDShooterSpeedsToSpeaker(Supplier pose, Supplier speeds, double dt) { + private Pair calculateSWDShooterSpeedsToSpeaker(Pose2d pose, ChassisSpeeds speeds, double dt) { Pose2d estimatedPose = integratePosition(pose, speeds, dt); SpeedAngleTriplet futureTriplet = @@ -373,7 +408,7 @@ private Pair calculateSWDShooterSpeedsToSpeaker(Supplier ); - double velocityNormalToSpeaker = getVelocityVectorToSpeaker(pose.get(), speeds.get()).getY(); + double velocityNormalToSpeaker = getVelocityVectorToSpeaker(pose, speeds).getY(); double averageAddedRPM = velocityToRPM(velocityNormalToSpeaker); double averageTotalRPM = futureTriplet.getAverageSpeed() - averageAddedRPM; @@ -389,7 +424,7 @@ private Pair calculateSWDShooterSpeedsToSpeaker(Supplier } - private Rotation2d calculateSWDPivotAngleToSpeaker(Supplier pose, Supplier speeds, double dt) { + private Rotation2d calculateSWDPivotAngleToSpeaker(Pose2d pose, ChassisSpeeds speeds, double dt) { Pose2d estimatedPose = integratePosition(pose, speeds, dt); estimatedPose = estimatedPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); @@ -397,9 +432,9 @@ private Rotation2d calculateSWDPivotAngleToSpeaker(Supplier pose, Suppli return Rotation2d.fromDegrees(ShooterConstants.INTERPOLATION_MAP.get(estimatedPose.getX()).getAngle()); } - private Pose2d integratePosition(Supplier pose, Supplier speeds, double dt) { - Pose2d robotPose = pose.get(); - ChassisSpeeds robotSpeeds = speeds.get(); + private Pose2d integratePosition(Pose2d pose, ChassisSpeeds speeds, double dt) { + Pose2d robotPose = pose; + ChassisSpeeds robotSpeeds = speeds; Pose2d estimatedPose = robotPose.exp(new Twist2d( robotSpeeds.vxMetersPerSecond * dt, From ef6bdfa9bfd876103a606b7625f3db396b9d38b5 Mon Sep 17 00:00:00 2001 From: Alexander Hamilton Date: Wed, 7 Feb 2024 00:52:51 -0800 Subject: [PATCH 2/9] add ability to make a speedangletriplet with a pair and angle --- src/main/java/frc/robot/util/SpeedAngleTriplet.java | 4 ++++ 1 file changed, 4 insertions(+) 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. From 838ae247680d7ee0adc0a9bd451e23d8fb1d0b53 Mon Sep 17 00:00:00 2001 From: Alexander Hamilton Date: Wed, 7 Feb 2024 00:53:33 -0800 Subject: [PATCH 3/9] Add the ability to calculate "ideal" rpm speeds based off of distance me when 120 mph at center line by scoring table :) --- .../java/frc/robot/commands/ShooterCalc.java | 54 +++++++++++++------ 1 file changed, 39 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index f925acc9..2784eaf6 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -7,6 +7,7 @@ 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; @@ -200,14 +201,34 @@ public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Supplier public Command prepareFireViaMathCommand(BooleanSupplier shootAtSpeaker, Supplier robotPose, Supplier speeds) { return Commands.run(() -> { - Rotation2d angle = calculateSWDPivotAngleToSpeaker(robotPose.get(), speeds.get(), 0.02); - Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), speeds.get(), 0.02); + Rotation2d angle = calculatePivotAngle(robotPose.get()); + Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), angle); pivot.setAngle(angle.getDegrees()); shooter.setSpeed(speedsPair); }, pivot, shooter); } + /** + * 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 Command prepareFireMovingCommand(Supplier robotPose, Supplier speeds) { return Commands.run(() -> { SpeedAngleTriplet triplet = calculateSWDSpeedAngleTripletToSpeaker(robotPose, speeds); @@ -363,23 +384,26 @@ public Command stopMotors() { * @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, ChassisSpeeds robotSpeeds, double dt) { - Rotation2d pivotAngle = calculateSWDPivotAngleToSpeaker(robotPose, robotSpeeds, dt); - double d = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()).getTranslation().getNorm(); - double desiredRPM = velocityToRPM(Math.sqrt(19.6*d)/(pivotAngle.getSin())); + private Pair calculateShooterSpeedsForApex(Pose2d robotPose, Rotation2d pivotAngle) { + double desiredRPM = velocityToRPM(Math.sqrt(2 * 9.8 * 2.08) / (pivotAngle.getSin())); return Pair.of(desiredRPM, desiredRPM); } 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()); + return Commands.runOnce( + () -> { + Rotation2d pivotAngle = calculatePivotAngle(pose.get()); + SpeedAngleTriplet calculationTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(pose.get(), pivotAngle), pivotAngle.getDegrees()); + + new NoteTrajectory( + pose, + speeds, + () -> Pair.of( + rpmToVelocity(calculationTriplet.getSpeeds()), + calculationTriplet.getAngle()) + ).schedule(); + } + ); } private SpeedAngleTriplet calculateSWDSpeedAngleTripletToSpeaker(Supplier pose, Supplier speeds){ From a185d29225842ad6ed7e2fe2484fff09380c9579 Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Wed, 7 Feb 2024 18:24:40 -0800 Subject: [PATCH 4/9] Works great on a singe axis (normal or tangental but NOT both) :D also rotate the speaker position to be a bit more normal --- .../java/frc/robot/commands/ShooterCalc.java | 44 ++++++++++++++----- src/main/java/frc/robot/util/Constants.java | 2 +- 2 files changed, 35 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 2784eaf6..8668b5d0 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -87,14 +87,20 @@ public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Supplier speeds) { /** - * Converts the velocity of the shooter wheel to RPM (Rotations Per Minute). + * Converts the velocity of the note to RPM (Rotations Per Minute). * Equation: ((V/(2π)) / (D/2)) * 60 = RPM * - * @param noteVelocity the velocity of the shooter wheel in meters per second + * @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 / diameter; + double radiansPerSecond = noteVelocity / (2*Math.PI); // Convert radians per second back to rotations per second - double rotationsPerSecond = radiansPerSecond / Math.PI; + double rotationsPerSecond = radiansPerSecond / (diameter/2); // Convert rotations per second back to rotations per minute return rotationsPerSecond * 60.0; @@ -393,7 +399,25 @@ public Command getNoteTrajectoryCommand(Supplier pose, Supplier { Rotation2d pivotAngle = calculatePivotAngle(pose.get()); - SpeedAngleTriplet calculationTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(pose.get(), pivotAngle), pivotAngle.getDegrees()); + Pose2d futurePose = integratePosition(pose.get(), speeds.get(), 0.02); + SpeedAngleTriplet currentTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(pose.get(), pivotAngle), pivotAngle.getDegrees()); + double normalVelocity = -getVelocityVectorToSpeaker(pose.get(), speeds.get()).getY(); + + double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); + double v0z = Math.sqrt(9.8*2*2.08); + double v0x = originalv0 * Math.cos(Math.toRadians(currentTriplet.getAngle())) + normalVelocity; + + double newv0 = Math.sqrt(Math.pow(v0x,2)+ Math.pow(v0z,2)); + Rotation2d newAngle = new Rotation2d(v0x, v0z); + + SpeedAngleTriplet calculationTriplet = + SpeedAngleTriplet.of( + Pair.of( + velocityToRPM(newv0), + velocityToRPM(newv0) + ), + newAngle.getDegrees() + ); new NoteTrajectory( pose, diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 8a5b09e7..1fa4a513 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -502,7 +502,7 @@ 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)), }; From 694c8776d9c7b7ec860c22943678e26b51e14698 Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Wed, 7 Feb 2024 20:59:01 -0800 Subject: [PATCH 5/9] Shoot while driving complete!!! Co-authored-by: Alexander Hamilton Co-authored-by: Jacob Hotz <77470805+Jacob1010-h@users.noreply.github.com> Co-authored-by: Rudy Good <122180943+RudyG252@users.noreply.github.com> --- .../java/frc/robot/commands/PieceControl.java | 1 - .../java/frc/robot/commands/ShooterCalc.java | 379 +++++++----------- 2 files changed, 144 insertions(+), 236 deletions(-) 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 8668b5d0..7b0af54c 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -9,12 +9,10 @@ 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; @@ -36,7 +34,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; @@ -60,126 +57,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) { - Translation2d velocityVectorToSpeaker = getVelocityVectorToSpeaker(robotPose, robotVelocity); - double velocityTangent = velocityVectorToSpeaker.getX(); - double velocityNormal = velocityVectorToSpeaker.getY(); - - 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 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 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; - } - /** * The function prepares a fire command by calculating the speed and angle for * the robot's shooter @@ -194,25 +76,22 @@ 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 prepareFireMovingCommand(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 Command prepareFireViaMathCommand(BooleanSupplier shootAtSpeaker, Supplier robotPose, Supplier speeds) { return Commands.run(() -> { - Rotation2d angle = calculatePivotAngle(robotPose.get()); - Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), angle); + Rotation2d angle = calculatePivotAngle(robotPose.get()); + Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), angle); - pivot.setAngle(angle.getDegrees()); - shooter.setSpeed(speedsPair); - }, pivot, shooter); + pivot.setAngle(angle.getDegrees()); + shooter.setSpeed(speedsPair); + }, pivot, shooter); } /** @@ -235,40 +114,6 @@ public Rotation2d calculatePivotAngle(Pose2d robotPose) { return new Rotation2d(distanceMeters, 3); } - public Command prepareFireMovingCommand(Supplier robotPose, Supplier speeds) { - return Commands.run(() -> { - SpeedAngleTriplet triplet = calculateSWDSpeedAngleTripletToSpeaker(robotPose, speeds); - - log(triplet); - - 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 void logAngles(SpeedAngleTriplet triplet) { - desiredAngle = -triplet.getAngle(); - realAngle = pivot.getAngle(); - } - - public void logAtDesired() { - atDesiredAngle = pivotAtDesiredAngle(); - atDesiredRPM = shooterAtDesiredRPM(); - } - - public void log(SpeedAngleTriplet triplet) { - logSpeeds(triplet); - logAngles(triplet); - logAtDesired(); - } /** * Determines if the pivot rotation is at its target with a small * tolerance @@ -283,7 +128,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 @@ -335,10 +179,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()); @@ -372,17 +212,118 @@ public boolean shooterAtDesiredRPM() { return atDesiredRPM().getAsBoolean(); } + + @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) { + Translation2d velocityVectorToSpeaker = getVelocityVectorToSpeaker(robotPose, robotVelocity); + double velocityTangent = velocityVectorToSpeaker.getX(); + + 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 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); + } + /** - * Stops the motors of the shooter and pivot subsystems. + * 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). * - * @return The command to stop the motors. + * 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 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 Command stopMotors() { - return Commands.parallel( - shooter.stop(), - pivot.stop()); + 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; } + /** * Calculates the shooter speeds required to reach the speaker position. * @@ -398,10 +339,10 @@ private Pair calculateShooterSpeedsForApex(Pose2d robotPose, Rot public Command getNoteTrajectoryCommand(Supplier pose, Supplier speeds) { return Commands.runOnce( () -> { - Rotation2d pivotAngle = calculatePivotAngle(pose.get()); - Pose2d futurePose = integratePosition(pose.get(), speeds.get(), 0.02); - SpeedAngleTriplet currentTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(pose.get(), pivotAngle), pivotAngle.getDegrees()); - double normalVelocity = -getVelocityVectorToSpeaker(pose.get(), speeds.get()).getY(); + Pose2d currentPose = pose.get(); + Rotation2d pivotAngle = calculatePivotAngle(currentPose); + SpeedAngleTriplet currentTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(currentPose, pivotAngle), pivotAngle.getDegrees()); + double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds.get()).getY(); double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); double v0z = Math.sqrt(9.8*2*2.08); @@ -413,14 +354,14 @@ public Command getNoteTrajectoryCommand(Supplier pose, Supplier Pair.of( rpmToVelocity(calculationTriplet.getSpeeds()), @@ -429,16 +370,6 @@ public Command getNoteTrajectoryCommand(Supplier pose, Supplier pose, Supplier speeds){ - double dt = DriverUI.currentTimestamp - DriverUI.previousTimestamp; - - return new SpeedAngleTriplet( - calculateSWDShooterSpeedsToSpeaker(pose.get(), speeds.get(), dt), - calculateSWDPivotAngleToSpeaker(pose.get(), speeds.get(), dt).getDegrees() - ); - } - /** * Calculates the shooter speeds required to reach the speaker position. * @@ -447,48 +378,26 @@ private SpeedAngleTriplet calculateSWDSpeedAngleTripletToSpeaker(Supplier calculateSWDShooterSpeedsToSpeaker(Pose2d pose, ChassisSpeeds speeds, double dt) { - Pose2d estimatedPose = integratePosition(pose, speeds, dt); - - SpeedAngleTriplet futureTriplet = - ShooterConstants.INTERPOLATION_MAP.get( - estimatedPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()).getX() + 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(9.8*2*2.08); + double v0x = originalv0 * Math.cos(Math.toRadians(currentTriplet.getAngle())) + normalVelocity; + + double newv0 = Math.sqrt(Math.pow(v0x,2)+ Math.pow(v0z,2)); + Rotation2d newAngle = new Rotation2d(v0x, v0z); + + return + SpeedAngleTriplet.of( + Pair.of( + velocityToRPM(newv0), + velocityToRPM(newv0) + ), + newAngle.getDegrees() ); - - - double velocityNormalToSpeaker = getVelocityVectorToSpeaker(pose, speeds).getY(); - double averageAddedRPM = velocityToRPM(velocityNormalToSpeaker); - - double averageTotalRPM = futureTriplet.getAverageSpeed() - averageAddedRPM; - - // 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(); - - return Pair.of( - averageTotalRPM + difference, - averageTotalRPM - difference - ); - } - - - private Rotation2d calculateSWDPivotAngleToSpeaker(Pose2d pose, ChassisSpeeds 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()); - } - - private Pose2d integratePosition(Pose2d pose, ChassisSpeeds speeds, double dt) { - Pose2d robotPose = pose; - ChassisSpeeds robotSpeeds = speeds; - - Pose2d estimatedPose = robotPose.exp(new Twist2d( - robotSpeeds.vxMetersPerSecond * dt, - robotSpeeds.vyMetersPerSecond * dt, - robotSpeeds.omegaRadiansPerSecond * dt) - ); - return estimatedPose; } } \ No newline at end of file From 2751db35bbe43b2cc8ecfbe3539f056768ad365d Mon Sep 17 00:00:00 2001 From: Alexander Hamilton Date: Wed, 7 Feb 2024 23:20:09 -0800 Subject: [PATCH 6/9] Refactor noteTrajectory to not have suppliers everywhere --- .../frc/robot/commands/NoteTrajectory.java | 45 +++++++++---------- .../java/frc/robot/commands/ShooterCalc.java | 7 ++- 2 files changed, 24 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/commands/NoteTrajectory.java b/src/main/java/frc/robot/commands/NoteTrajectory.java index b6baefb3..d0f13c61 100644 --- a/src/main/java/frc/robot/commands/NoteTrajectory.java +++ b/src/main/java/frc/robot/commands/NoteTrajectory.java @@ -24,35 +24,34 @@ 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,10 +60,9 @@ public void initialize() { x0 = NTConstants.PIVOT_OFFSET_METERS.getX(); y0 = 0; z0 = NTConstants.PIVOT_OFFSET_METERS.getY(); - vx0 = Rotation2d.fromDegrees(90 - speedAnglePair.getSecond()).getSin() * speedAnglePair.getFirst() - + speeds.vxMetersPerSecond; - vy0 = speeds.vyMetersPerSecond; - vz0 = Rotation2d.fromDegrees(90 - speedAnglePair.getSecond()).getCos() * 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; @@ -78,9 +76,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 +98,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 +119,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/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 7b0af54c..546bba80 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -362,10 +362,9 @@ public Command getNoteTrajectoryCommand(Supplier pose, Supplier Pair.of( - rpmToVelocity(calculationTriplet.getSpeeds()), - calculationTriplet.getAngle()) + speeds.get(), + rpmToVelocity(calculationTriplet.getSpeeds()), + calculationTriplet.getAngle() ).schedule(); } ); From 6298e9396dbaa8cd4b0059d3ab8ed695db52b8ba Mon Sep 17 00:00:00 2001 From: Jacob Hotz Date: Thu, 8 Feb 2024 17:23:33 -0800 Subject: [PATCH 7/9] resolve changes :DD --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/NoteTrajectory.java | 3 +- .../java/frc/robot/commands/ShooterCalc.java | 34 +++++-------------- src/main/java/frc/robot/util/Constants.java | 4 ++- 4 files changed, 14 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0582902a..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.prepareFireViaMathCommand(() -> true, swerve::getPose, swerve::getRobotRelativeVelocity)); + .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 d0f13c61..b4fa841e 100644 --- a/src/main/java/frc/robot/commands/NoteTrajectory.java +++ b/src/main/java/frc/robot/commands/NoteTrajectory.java @@ -18,6 +18,7 @@ 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 { @@ -65,7 +66,7 @@ public void initialize() { 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. diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 546bba80..03f27682 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -18,6 +18,7 @@ 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 { @@ -76,7 +77,7 @@ public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Supplier`. * @return The method is returning a Command object. */ - public Command prepareFireMovingCommand(Supplier robotPose, Supplier speeds) { + public Command prepareSWDCommand(Supplier robotPose, Supplier speeds) { return Commands.run(() -> { SpeedAngleTriplet triplet = calculateSWDTriplet(robotPose.get(), speeds.get()); pivot.setAngle(triplet.getAngle()); @@ -84,7 +85,7 @@ public Command prepareFireMovingCommand(Supplier robotPose, Supplier robotPose, Supplier speeds) { + public Command prepareSWDSimCommand(BooleanSupplier shootAtSpeaker, Supplier robotPose, Supplier robotSpeeds) { return Commands.run(() -> { Rotation2d angle = calculatePivotAngle(robotPose.get()); Pair speedsPair = calculateShooterSpeedsForApex(robotPose.get(), angle); @@ -332,33 +333,14 @@ public double velocityToRPM(double noteVelocity) { * @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 * 9.8 * 2.08) / (pivotAngle.getSin())); + double desiredRPM = velocityToRPM(Math.sqrt(2 * Constants.GRAVITY * 2.08) / (pivotAngle.getSin())); return Pair.of(desiredRPM, desiredRPM); } public Command getNoteTrajectoryCommand(Supplier pose, Supplier speeds) { return Commands.runOnce( () -> { - Pose2d currentPose = pose.get(); - Rotation2d pivotAngle = calculatePivotAngle(currentPose); - SpeedAngleTriplet currentTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(currentPose, pivotAngle), pivotAngle.getDegrees()); - double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds.get()).getY(); - - double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); - double v0z = Math.sqrt(9.8*2*2.08); - double v0x = originalv0 * Math.cos(Math.toRadians(currentTriplet.getAngle())) + normalVelocity; - - double newv0 = Math.sqrt(Math.pow(v0x,2)+ Math.pow(v0z,2)); - Rotation2d newAngle = new Rotation2d(v0x, v0z); - - SpeedAngleTriplet calculationTriplet = - SpeedAngleTriplet.of( - Pair.of( - velocityToRPM(newv0), - velocityToRPM(newv0) - ), - newAngle.getDegrees() - ); + SpeedAngleTriplet calculationTriplet = calculateSWDTriplet(pose.get(), speeds.get()); new NoteTrajectory( pose, @@ -384,10 +366,10 @@ private SpeedAngleTriplet calculateSWDTriplet(Pose2d pose, ChassisSpeeds speeds) double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds).getY(); double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); - double v0z = Math.sqrt(9.8*2*2.08); - double v0x = originalv0 * Math.cos(Math.toRadians(currentTriplet.getAngle())) + normalVelocity; + double v0z = Math.sqrt(Constants.GRAVITY*2*2.08); + double v0x = originalv0 * Math.cos(Units.degreesToRadians(currentTriplet.getAngle())) + normalVelocity; - double newv0 = Math.sqrt(Math.pow(v0x,2)+ Math.pow(v0z,2)); + double newv0 = Math.hypot(v0x, v0z); Rotation2d newAngle = new Rotation2d(v0x, v0z); return diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 1fa4a513..4a0ae7d2 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -504,7 +504,7 @@ public static enum GameMode { // Blue Speaker 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)), }; private static final Pose2d[] AMP_POSITIONS = new Pose2d[] { @@ -543,4 +543,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 From 1b3e4c2e5c4176d956d6d8a0d017270a748492c6 Mon Sep 17 00:00:00 2001 From: Jacob Hotz Date: Fri, 9 Feb 2024 10:43:46 -0800 Subject: [PATCH 8/9] fix all but negative sign? --- src/main/java/frc/robot/commands/NoteTrajectory.java | 2 +- src/main/java/frc/robot/commands/ShooterCalc.java | 4 ++-- src/main/java/frc/robot/util/Constants.java | 4 +++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/NoteTrajectory.java b/src/main/java/frc/robot/commands/NoteTrajectory.java index b4fa841e..4529128b 100644 --- a/src/main/java/frc/robot/commands/NoteTrajectory.java +++ b/src/main/java/frc/robot/commands/NoteTrajectory.java @@ -66,7 +66,7 @@ public void initialize() { vz0 = Rotation2d.fromDegrees(pivotAngle).getSin() * initialVelocity; ax = 0; ay = 0; - az = -Constants.GRAVITY; + az = Constants.GRAVITY; } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 03f27682..524012db 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -333,7 +333,7 @@ public double velocityToRPM(double noteVelocity) { * @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 * 2.08) / (pivotAngle.getSin())); + double desiredRPM = velocityToRPM(Math.sqrt(2 * -Constants.GRAVITY * FieldConstants.SPEAKER_HEIGHT) / (pivotAngle.getSin())); return Pair.of(desiredRPM, desiredRPM); } @@ -366,7 +366,7 @@ private SpeedAngleTriplet calculateSWDTriplet(Pose2d pose, ChassisSpeeds speeds) double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds).getY(); double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); - double v0z = Math.sqrt(Constants.GRAVITY*2*2.08); + 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); diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 4a0ae7d2..2567c861 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -507,6 +507,8 @@ public static enum GameMode { 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,6 +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; + public static final double GRAVITY = -9.8; } \ No newline at end of file From 380803d7b25ec1077f4d29941eed7c67d772d055 Mon Sep 17 00:00:00 2001 From: Jacob Hotz Date: Fri, 9 Feb 2024 10:56:45 -0800 Subject: [PATCH 9/9] undo gravity --- src/main/java/frc/robot/commands/NoteTrajectory.java | 2 +- src/main/java/frc/robot/commands/ShooterCalc.java | 4 ++-- src/main/java/frc/robot/util/Constants.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/NoteTrajectory.java b/src/main/java/frc/robot/commands/NoteTrajectory.java index c8a3b2b7..940cc24d 100644 --- a/src/main/java/frc/robot/commands/NoteTrajectory.java +++ b/src/main/java/frc/robot/commands/NoteTrajectory.java @@ -68,7 +68,7 @@ public void initialize() { ax = 0; ay = 0; - az = Constants.GRAVITY; + az = -Constants.GRAVITY; } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 524012db..450462c9 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -333,7 +333,7 @@ public double velocityToRPM(double noteVelocity) { * @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())); + double desiredRPM = velocityToRPM(Math.sqrt(2 * Constants.GRAVITY * FieldConstants.SPEAKER_HEIGHT) / (pivotAngle.getSin())); return Pair.of(desiredRPM, desiredRPM); } @@ -366,7 +366,7 @@ private SpeedAngleTriplet calculateSWDTriplet(Pose2d pose, ChassisSpeeds speeds) double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds).getY(); double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); - double v0z = Math.sqrt(-Constants.GRAVITY*2*2.08); + 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); diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 2567c861..0af2aceb 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -545,6 +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; + public static final double GRAVITY = 9.8; } \ No newline at end of file