Flywheel AdvantageKit

src/main/java/org/team157/robot/RobotContainer.java

@@ -28,7 +28,8 @@
 import org.team157.robot.Constants.ModifierConstants;
 import org.team157.robot.generated.TunerConstants;
 import org.team157.robot.subsystems.drive.DriveSystem;
-import org.team157.robot.subsystems.flywheel.FlywheelSystem;
+import org.team157.robot.subsystems.flywheel.Flywheel;
+import org.team157.robot.subsystems.flywheel.FlywheelIOTalonFX;
 import org.team157.robot.subsystems.hood.Hood;
 import org.team157.robot.subsystems.hood.HoodIOTalonFX;
 import org.team157.robot.subsystems.hopper.Hopper;
@@ -52,7 +53,6 @@ public class RobotContainer {
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
     private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
-    private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
     private final Telemetry logger = new Telemetry(MaxSpeed);
 
@@ -61,7 +61,7 @@ public class RobotContainer {
 
     public final Turret turret = new Turret();
     public final VisionSystem visionSystem;
-    public final FlywheelSystem flywheel = new FlywheelSystem();
+    public static final Flywheel flywheel = new Flywheel();
     // TODO: consider whether other systems should be static, had to make this static for the dyanmic hood under trench.
     public static final DriveSystem drivetrain = TunerConstants.createDrivetrain();
 
@@ -91,7 +91,7 @@ public RobotContainer() {
         slapdown.setIO(new SlapdownIOTalonFX(slapdown));
         hopper.setIO(new HopperIOTalonFX(hopper));
         uptake.setIO(new UptakeIOTalonFX(uptake));
-
+        flywheel.setIO(new FlywheelIOTalonFX(flywheel));
         visionSystem = new VisionSystem(drivetrain::getPose, Robot.m_field);
         turret.setIO(new TurretIOTalonFX(turret), visionSystem);
 
@@ -128,7 +128,7 @@ private void configureBindings() {
 
         // Disable turret movement when no other turret commands are running.
         turret.setDefaultCommand(turret.getDefault());
-        flywheel.setDefaultCommand(flywheel.setDefault());
+        flywheel.setDefaultCommand(flywheel.getDefault());
         slapdown.setDefaultCommand(slapdown.getDefault());
         intake.setDefaultCommand(intake.getDefault());
         hopper.setDefaultCommand(hopper.getDefault());

src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java

@@ -1,5 +1,218 @@
 package org.team157.robot.subsystems.flywheel;
 
-public class Flywheel {
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.Radians;
 
+import org.littletonrobotics.junction.Logger;
+import org.team157.robot.Constants.FieldConstants;
+import org.team157.robot.RobotContainer;
+import org.team157.robot.subsystems.hood.HoodConstants;
+import org.team157.robot.subsystems.vision.VisionSystem;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Represents the Flywheel subsystem, which spins up to
+ * launch balls at a calculated velocity towards the target.
+ */
+public class Flywheel extends SubsystemBase {
+
+    // The IO interface for interacting with the flywheel's motors.
+    private FlywheelIO io;
+
+    // Inputs from the motors and mechanism, to be updated periodically and logged.
+    private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
+
+    /** The calculated ball velocity in meters per second required for the current shot. */
+    public static double ballVelocity = 0;
+
+    /** The calculated hood angle in radians required for the current shot. */
+    public static Angle hoodAngle = Radians.of(0);
+
+    /** Creates a new Flywheel. */
+    public Flywheel() {}
+
+    /**
+     * Specifies the IO implementation to be used for the Flywheel.
+     *
+     * @param io An implementation of the Flywheel's IO layer, i.e. FlywheelIOTalonFX
+     */
+    public void setIO(FlywheelIO io) {
+        this.io = io;
+    }
+
+      /////////////////////////
+     /// FLYWHEEL COMMANDS ///
+    /////////////////////////
+
+    /**
+     * Sets the default command of the flywheel, stopping
+     * motor output when no other commands are running.
+     *
+     * @return Command setting the duty cycle output of the flywheel's motor to 0
+     */
+    public Command getDefault() {
+        return io.set(0);
+    }
+
+    /**
+     * Set the duty cycle output of the flywheel motor.
+     *
+     * @param dutyCycle The power to be applied to the motor, between -1 and 1.
+     * @return {@link Command} setting the duty cycle of the flywheel.
+     */
+    public Command set(double dutyCycle) {
+        return io.set(dutyCycle);
+    }
+
+    /**
+     * Set the flywheel to a fixed target angular velocity.
+     *
+     * @param speed The target angular velocity.
+     * @return {@link Command} setting the flywheel to the specified velocity.
+     */
+    public Command setVelocity(AngularVelocity speed) {
+        return io.setVelocity(speed);
+    }
+
+    /**
+     * Set the flywheel to a dynamically-calculated velocity based on
+     * the current distance and height to the target.
+     *
+     * @return {@link Command} continuously updating the flywheel velocity.
+     */
+    public Command setDynamicVelocity() {
+        return io.setVelocity(this::getDesiredFlywheelVelocity);
+    }
+
+      ////////////////////////
+     /// FLYWHEEL METHODS ///
+    ////////////////////////
+
+    /**
+     * Gets the current velocity of the flywheel.
+     *
+     * @return The current angular velocity of the flywheel.
+     */
+    public AngularVelocity getVelocity() {
+        return RPM.of(inputs.mechanismVelocityRPM);
+    }
+
+      ////////////////////////////////////////////
+     /////// DYNAMIC VELOCITY CALCULATION ///////
+    ////////////////////////////////////////////
+
+    /**   Calculates required ball velocity (m/s)
+     * for given distance, height, and launch angle
+     * <br>
+     * Using projectile motion equations: y = x*tan(θ) - (g*x²)/(2*v₀²*cos²(θ))
+     * <br>
+     * Solving for v₀: v₀ = sqrt((g*x²)/(2*cos²(θ)*(x*tan(θ) - y)))
+     * 
+     * @param distance The horizontal distance to the target in meters.
+     * @param height   The vertical height of the target in meters.
+     * @param theta    The launch angle in radians.
+     * @return The required ball velocity in meters per second.
+     */
+    static double velocityFunction(double distance, double height, double theta) {
+        double g = 9.81;
+        double heightDifference = height - FlywheelConstants.HEIGHT.magnitude();
+        double denominator = 2 * Math.cos(theta) * Math.cos(theta) * (distance * Math.tan(theta) - heightDifference);
+        if (denominator <= 0) {
+            return 157; // Invalid shot parameters, return arbitrary velocity
+        }
+        return Math.sqrt((g * distance * distance) / denominator);
+    }
+
+    /**
+     * Determines the lower bound of the hood angle search space based on whether the robot is under the trench,
+     * since the hood must avoid collisions with the trench structure.
+     * @param isUnderTrench Whether or not the turret is currently under the trench, determined by the drivetrain's position on the field.
+     * @return The lower bound of the hood angle search space, in radians.
+     */
+    double getLowerBound(boolean isUnderTrench) {
+        if (isUnderTrench) {
+            return HoodConstants.UPPER_SOFT_LIMIT.in(Radians);
+        } else {
+            return HoodConstants.LOWER_SOFT_LIMIT.in(Radians);
+        }
+    }
+
+    /**
+     * Calculates and updates the optimal flywheel velocity and hood angle
+     * for the current shot based on target distance and height.
+     *
+     * @param height   Target height in meters.
+     * @param distance Distance to target in meters.
+     */
+    public void setShotParams(double height, double distance) {
+        double lowerBound = getLowerBound(RobotContainer.drivetrain.isUnderTrench());
+        double upperBound = HoodConstants.UPPER_SOFT_LIMIT.in(Radians);
+        double steps = 50;
+        double stepSize = (upperBound - lowerBound) / steps;
+
+        double theta = lowerBound;
+        double velocity = velocityFunction(distance, height, lowerBound);
+
+        for (int i = 1; i <= steps; i++) {
+            double x = lowerBound + i * stepSize;
+            double y = velocityFunction(distance, height, x);
+            if (y < velocity) {
+                velocity = y;
+                theta = x;
+            }
+        }
+
+        ballVelocity = velocity;
+        hoodAngle = Radians.of(theta);
+    }
+
+    /**
+     * Gets the desired flywheel velocity for the current shot,
+     * recalculating shot parameters each time it is called.
+     *
+     * @return The desired angular velocity of the flywheel.
+     */
+    public AngularVelocity getDesiredFlywheelVelocity() {
+        double heightMeters = FieldConstants.positionDetails.getTargetHeight();
+        double distanceMeters = VisionSystem.distanceToTargetFromTurret;
+
+        setShotParams(heightMeters, distanceMeters);
+
+        // Convert ball velocity (m/s) to flywheel RPM:
+        // flywheelRPM = (ballVelocity * 60) / (π * flywheel_diameter)
+        // divided by SPEED_FACTOR to account for air resistance and wheel slip
+        double flywheelDiameterMeters = FlywheelConstants.FLYWHEEL_DIAMETER.in(Meters);
+        double desiredRPM = (ballVelocity * 60) / (Math.PI * flywheelDiameterMeters) * FlywheelConstants.SPEED_FACTOR;
+        return RPM.of(Math.max(2800, desiredRPM));
+    }
+
+    /**
+     * Gets the desired hood angle for the current shot, as calculated
+     * by the most recent call to {@link #setShotParams}.
+     *
+     * @return The desired hood angle.
+     */
+    public static Angle getDesiredHoodAngle() {
+        return Degrees.of(Math.toDegrees(hoodAngle.magnitude()));
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+        // Updates the inputs to be logged by AdvantageKit and writes them to the Logger
+        io.updateInputs(inputs, getDesiredFlywheelVelocity());
+        Logger.processInputs("Flywheel", inputs);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+        io.simIterate();
+    }
 }

src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIO.java

@@ -1,5 +1,74 @@
 package org.team157.robot.subsystems.flywheel;
 
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+/**
+ * Defines the input data to be logged by AdvantageKit, along with methods and
+ * {@link Command}s which an implementation of this IO interface must have.
+ */
 public interface FlywheelIO {
 
+    /**
+     * Represents the set of inputs which are to be logged by AdvantageKit
+     * and updated by an implementation of the {@link FlywheelIO} interface.
+     */
+    @AutoLog
+    public static class FlywheelIOInputs {
+        public double supplyCurrentAmps = 0.0;
+        public double statorCurrentAmps = 0.0;
+        public double appliedVolts = 0.0;
+        public double temperatureCelsius = 0.0;
+        public double mechanismVelocityRPM = 0.0;
+        public double targetVelocityRPM = 0.0;
+    }
+
+    /**
+     * Updates the inputs to be logged by AdvantageKit.
+     *
+     * @param inputs The set of inputs to be logged, including information on the motor and mechanism.
+     * @param flywheelSetpoint The current target angular velocity of the flywheel, used for logging the error between the target and actual velocities.
+     */
+    default void updateInputs(FlywheelIOInputs inputs, AngularVelocity flywheelSetpoint) {}
+
+    /** Updates the values for the simulated version of the flywheel mechanism. */
+    default void simIterate() {}
+
+    /** Stops the flywheel. */
+    default void stop() {}
+
+    /**
+     * Directly sets the output duty cycle of the flywheel motor.
+     *
+     * @param dutyCycle The duty cycle to apply to the motor, between -1 and 1.
+     * @return a {@link Command} setting the motor's duty cycle to the specified value.
+     */
+    default Command set(double dutyCycle) {
+        return Commands.none();
+    }
+
+    /**
+     * Sets the flywheel to a fixed target angular velocity.
+     *
+     * @param velocity The target angular velocity.
+     * @return a {@link Command} setting the flywheel to the specified velocity.
+     */
+    default Command setVelocity(AngularVelocity velocity) {
+        return Commands.none();
+    }
+
+    /**
+     * Sets the flywheel to a dynamically-supplied target angular velocity.
+     *
+     * @param velocity A {@link Supplier} providing the target angular velocity.
+     * @return a {@link Command} continuously updating the flywheel velocity from the supplier.
+     */
+    default Command setVelocity(Supplier<AngularVelocity> velocity) {
+        return Commands.none();
+    }
 }