Add note trajectory and visualization (#110)

src/main/java/frc/robot/RobotContainer.java

@@ -6,6 +6,7 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.revrobotics.CANSparkBase;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -19,6 +20,7 @@
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.util.PatriBoxController;
 import frc.robot.util.PoseCalculations;
+import frc.robot.util.SpeedAngleTriplet;
 import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.NeoMotorConstants;
 import frc.robot.util.Constants.OIConstants;
@@ -119,9 +121,12 @@ private void configureDriverBindings() {
         driver.a().and(intake.hasGamePieceTrigger().negate()).onTrue(intake.inCommand());
 
         driver.y().onTrue(intake.outCommand());
-
+        
         driver.x().onTrue(intake.stop());
 
+        // TODO: change the SpeedAnglePair to have the corresponding values from shooter calc
+        driver.a().onTrue(shooterCalc.getNoteTrajectoryCommand(swerve::getPose, new SpeedAngleTriplet(12.0, 12.0, 45.0)));
+
     }
 
     public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/NoteTrajectory.java

@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.util.SpeedAngleTriplet;
+import monologue.Logged;
+import monologue.Annotations.Log;
+
+/**
+ * Calculates the trajectory of an object in a 2D plane.
+ * 
+ * @param endX The x-coordinate at which the trajectory should end. Set to -1 to
+ *             calculate until the object reaches the ground.
+ * @return An ArrayList of Pose3d objects representing the trajectory of the
+ *         object.
+ */
+public class NoteTrajectory implements Logged {
+    DoubleSupplier x;
+    DoubleSupplier y;
+    DoubleSupplier x0;
+    DoubleSupplier y0;
+    DoubleSupplier vx0;
+    DoubleSupplier vy0;
+    DoubleSupplier ax;
+    DoubleSupplier ay;
+
+    Supplier<Pose2d> initialPose;
+
+    Timer timer;
+
+    @Log.NT
+    Pose3d traj = new Pose3d();
+
+    public NoteTrajectory() {
+        setVariables(new Pose2d(), new SpeedAngleTriplet());
+    }
+
+    private void setVariables(Pose2d initialPose2d, SpeedAngleTriplet speedAngleTriplet) {
+        x = () -> 0;
+        y = () -> 0;
+        x0 = () -> 0;
+        y0 = () -> 0;
+        // TODO: Change the left and right values to not be the x and y components as
+        // they are the wrong axis
+        vx0 = () -> Rotation2d.fromDegrees(90 - (speedAngleTriplet.getAngle())).getSin()
+                * speedAngleTriplet.getLeftSpeed();
+        vy0 = () -> Rotation2d.fromDegrees(90 - (speedAngleTriplet.getAngle())).getCos()
+                * speedAngleTriplet.getRightSpeed();
+        ax = () -> 0;
+        ay = () -> -9.8;
+        initialPose = () -> initialPose2d;
+    }
+
+    public Command getNoteTrajectoryCommand(Supplier<Pose2d> pose, SpeedAngleTriplet speedAngleTriplet) {
+        return Commands.runOnce(() -> {
+            this.timer = new Timer();
+            this.timer.start();
+            setVariables(pose.get(), speedAngleTriplet);
+        }).andThen(Commands.runOnce(() -> {
+            x = kinematicEquation1(x0, vx0, ax, timer);
+            y = kinematicEquation1(x0, vy0, ay, timer);
+
+            traj = getNotePose(this.initialPose, x, y);
+        }).repeatedly().until(() -> ((y.getAsDouble() < y0.getAsDouble()) || timer.get() > 2.5)));
+    }
+
+    Pose3d getNotePose(Supplier<Pose2d> pose, DoubleSupplier x, DoubleSupplier y) {
+        return new Pose3d(
+                new Translation3d(x.getAsDouble(), 0, y.getAsDouble()).rotateBy(new Rotation3d(
+                        0,
+                        0,
+                        pose.get().getRotation().getRadians())),
+                new Rotation3d())
+                .transformBy(
+                        new Transform3d(
+                                new Translation3d(
+                                        pose.get().getX(),
+                                        pose.get().getY(),
+                                        0),
+                                new Rotation3d()));
+    }
+
+    /**
+     * Calculates the position using the kinematic equation:
+     * x = x0 + v0 * t + 0.5 * a * t^2
+     *
+     * @param x0 the initial position
+     * @param v0 the initial velocity
+     * @param a  the acceleration
+     * @param t  the time
+     * @return the final position
+     */
+    public DoubleSupplier kinematicEquation1(DoubleSupplier x0, DoubleSupplier v0, DoubleSupplier a, Timer t) {
+        DoubleSupplier x = () -> (x0.getAsDouble() + v0.getAsDouble() * t.get()
+                + 0.5 * a.getAsDouble() * Math.pow(t.get(), 2));
+        return x;
+    }
+}

src/main/java/frc/robot/commands/ShooterCalc.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
+import java.util.function.Supplier;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.Swerve;
@@ -24,12 +25,15 @@ public class ShooterCalc implements Logged {
     private Shooter shooter;
     private boolean aiming;
 
+    private NoteTrajectory noteTrajectory;
+
     public ShooterCalc(Shooter shooter, Pivot pivot) {
         this.pivot = pivot;
         this.shooter = shooter;
         this.aiming = false;
+        this.noteTrajectory = noteTrajectory;
     }
-
+    
     /**
      * The function prepares a fire command by calculating the speed and angle for
      * the robot's shooter
@@ -45,18 +49,19 @@ public ShooterCalc(Shooter shooter, Pivot pivot) {
      */
     public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) {
         SpeedAngleTriplet triplet = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean());
-
+        
         return pivot.setAngleCommand(triplet.getAngle())
-                .alongWith(shooter.setSpeedCommand(triplet.getSpeeds()));
+            .alongWith(shooter.setSpeedCommand(triplet.getSpeeds()));
     }
-
+    
     @Log.NT
     Rotation2d currentAngleToSpeaker;
     @Log.NT
     Pose2d robotPoseAngled;
     @Log.NT
     Pose2d robotPoseSurely;
 
+
     /**
      * 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.
@@ -86,12 +91,52 @@ public Rotation2d calculateSWDAngleToSpeaker(Pose2d robotPose, ChassisSpeeds rob
         return new Rotation2d(velocityTranslation.getX(), velocityTranslation.getY());
     }
 
+    /**
+     * TODO: These should both probably work off of the current speed, not the desired speed.
+     * 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<Double, Double> speeds) {
-        return 12;
+
+        double rotationsPerMinute = (speeds.getFirst() + speeds.getSecond()) / 2.0;
+        double rotationsPerSecond = rotationsPerMinute / 60.0;
+        double radiansPerSecond = rotationsPerSecond * Math.PI;
+
+        double diameter = ShooterConstants.WHEEL_DIAMETER_METERS;
+
+        // Normally this is 1 radius * 2 pi
+        // but we are doing 2 radius * 1 pi
+        // because we are given a diameter
+        return diameter * radiansPerSecond;
     }
 
-    public double velocityToRPM(double velocity) {
-        return 1;
+
+    /**
+     * 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;
     }
 
     /**
@@ -225,17 +270,36 @@ private SpeedAngleTriplet calculateSpeed(Pose2d robotPose, boolean shootingAtSpe
         return ShooterConstants.INTERPOLATION_MAP.get(distanceFeet);
     }
 
+    /**
+     * Checks if the pivot is at the desired angle.
+     * 
+     * @return true if the pivot is at the desired angle, false otherwise.
+     */
     public boolean pivotAtDesiredAngle() {
         return pivot.atDesiredAngle().getAsBoolean();
     }
 
+    /**
+     * Checks if the shooter is at the desired RPM.
+     * 
+     * @return true if the shooter is at the desired RPM, false otherwise
+     */
     public boolean shooterAtDesiredRPM() {
         return shooter.atDesiredRPM().getAsBoolean();
     }
 
+    /**
+     * Stops the motors of the shooter and pivot subsystems.
+     * 
+     * @return The command to stop the motors.
+     */
     public Command stopMotors() {
         return Commands.parallel(
                 shooter.stop(),
                 pivot.stop());
     }
+
+    public Command getNoteTrajectoryCommand(Supplier<Pose2d> pose, SpeedAngleTriplet speedAngleTriplet) {
+        return noteTrajectory.getNoteTrajectoryCommand(pose, speedAngleTriplet);
+    }
 }

src/main/java/frc/robot/util/Constants.java

@@ -143,6 +143,8 @@ public static final class ShooterConstants {
             }
         };
 
+        public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(3);
+
     }
 
     public static final class TrapConstants {