Split feeder and flywheels

Mechanical-Advantage · Feb 9, 2024 · ce05966 · ce05966
1 parent f94a498
commit ce05966
Show file tree

Hide file tree

Showing 21 changed files with 526 additions and 702 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -35,13 +35,13 @@
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim;
+import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels;
+import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO;
+import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim;
+import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSparkFlex;
 import org.littletonrobotics.frc2024.subsystems.superstructure.intake.Intake;
 import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIO;
 import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOSim;
-import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.Shooter;
-import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.ShooterIO;
-import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.ShooterIOSim;
-import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.ShooterIOSparkMax;
 import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
 import org.littletonrobotics.frc2024.util.trajectory.ChoreoTrajectoryDeserializer;
 import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory;
@@ -60,7 +60,7 @@ public class RobotContainer {
     // Subsystems
     private Drive drive;
     private AprilTagVision aprilTagVision;
-    private Shooter shooter;
+    private Flywheels flywheels;
     private Intake intake;
     private Arm arm;
     private DevBotSuperstructure superstructure;
@@ -83,9 +83,9 @@ public RobotContainer() {
                                 new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]),
                                 new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]));
                 arm = new Arm(new ArmIOKrakenFOC());
-                shooter = new Shooter(new ShooterIOSparkMax());
+                flywheels = new Flywheels(new FlywheelsIOSparkFlex());
                 //        intake = new Intake(new IntakeIOSparkMax());
-                superstructure = new DevBotSuperstructure(arm, shooter);
+                superstructure = new DevBotSuperstructure(arm, flywheels);
             }
             case SIMBOT -> {
                 drive =
@@ -96,9 +96,9 @@ public RobotContainer() {
                                 new ModuleIOSim(DriveConstants.moduleConfigs[2]),
                                 new ModuleIOSim(DriveConstants.moduleConfigs[3]));
                 arm = new Arm(new ArmIOSim());
-                shooter = new Shooter(new ShooterIOSim());
+                flywheels = new Flywheels(new FlywheelsIOSim());
                 intake = new Intake(new IntakeIOSim());
-                superstructure = new DevBotSuperstructure(arm, shooter);
+                superstructure = new DevBotSuperstructure(arm, flywheels);
             }
             case COMPBOT -> {
                 // No impl yet
@@ -119,8 +119,8 @@ public RobotContainer() {
             aprilTagVision = new AprilTagVision();
         }
 
-        if (shooter == null) {
-            shooter = new Shooter(new ShooterIO() {});
+        if (flywheels == null) {
+            flywheels = new Flywheels(new FlywheelsIO() {});
         }
 
         if (intake == null) {
@@ -141,15 +141,15 @@ public RobotContainer() {
         autoChooser.addOption(
                 "Left Flywheels FF Characterization",
                 new FeedForwardCharacterization(
-                        shooter,
-                        shooter::runLeftCharacterizationVolts,
-                        shooter::getLeftCharacterizationVelocity));
+                        flywheels,
+                        flywheels::runLeftCharacterizationVolts,
+                        flywheels::getLeftCharacterizationVelocity));
         autoChooser.addOption(
                 "Right Flywheels FF Characterization",
                 new FeedForwardCharacterization(
-                        shooter,
-                        shooter::runRightCharacterizationVolts,
-                        shooter::getRightCharacterizationVelocity));
+                        flywheels,
+                        flywheels::runRightCharacterizationVolts,
+                        flywheels::getRightCharacterizationVelocity));
         autoChooser.addOption("Arm get static current", arm.getStaticCurrent());
 
         // Testing autos paths

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java
@@ -1,5 +1,8 @@
 package org.littletonrobotics.frc2024.subsystems.drive;
 
+import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.driveConfig;
+import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.moduleConstants;
+
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -8,9 +11,6 @@
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.Logger;
 
-import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.moduleConstants;
-import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.driveConfig;
-
 public class Module {
     private static final LoggedTunableNumber drivekP =
             new LoggedTunableNumber("Drive/Module/DrivekP", moduleConstants.drivekP());
@@ -29,8 +29,7 @@ public class Module {
     private final ModuleIO io;
     private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
     private SimpleMotorFeedforward driveFF =
-            new SimpleMotorFeedforward(
-                    moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0);
+            new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0);
 
     @Getter private SwerveModuleState setpointState = new SwerveModuleState();
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java
@@ -171,7 +171,8 @@ public void updateInputs(ModuleIOInputs inputs) {
 
         inputs.odometryDrivePositionsMeters =
                 drivePositionQueue.stream()
-                        .mapToDouble(signalValue -> Units.rotationsToRadians(signalValue) * driveConfig.wheelRadius())
+                        .mapToDouble(
+                                signalValue -> Units.rotationsToRadians(signalValue) * driveConfig.wheelRadius())
                         .toArray();
         inputs.odometryTurnPositions =
                 turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new);

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java
@@ -94,8 +94,7 @@ public ModuleIOSparkMax(ModuleConfig config) {
         drivePositionQueue =
                 SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition);
         turnPositionQueue =
-                SparkMaxOdometryThread.getInstance()
-                        .registerSignal(absoluteEncoderValue.get()::getRadians);
+                SparkMaxOdometryThread.getInstance().registerSignal(absoluteEncoderValue.get()::getRadians);
 
         // Init Controllers
         driveController = new PIDController(moduleConstants.drivekP(), 0.0, moduleConstants.drivekD());

diff --git a/.../java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/.../java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java
@@ -24,7 +24,8 @@ public static void main(String[] args) {
                         .setWheelRadius(DriveConstants.driveConfig.wheelRadius())
                         .setMaxWheelTorque(2)
                         .setMaxWheelOmega(
-                                DriveConstants.moduleLimits.maxDriveVelocity() / DriveConstants.driveConfig.wheelRadius())
+                                DriveConstants.moduleLimits.maxDriveVelocity()
+                                        / DriveConstants.driveConfig.wheelRadius())
                         .build();
 
         PathRequest request =

diff --git a/...in/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java b/...in/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java
@@ -1,13 +1,10 @@
 package org.littletonrobotics.frc2024.subsystems.superstructure;
 
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import lombok.Getter;
-import org.littletonrobotics.frc2024.FieldConstants;
-import org.littletonrobotics.frc2024.RobotState;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
-import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.Shooter;
+import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -39,46 +36,23 @@ public enum State {
 
     @Getter private State currentState = State.IDLE;
     private final Arm arm;
-    private final Shooter shooter;
+    private final Flywheels flywheels;
 
     private final Timer followThroughTimer = new Timer();
     private double followThroughArmAngle = 0.0;
 
-    public DevBotSuperstructure(Arm arm, Shooter shooter) {
+    public DevBotSuperstructure(Arm arm, Flywheels flywheels) {
         this.arm = arm;
-        this.shooter = shooter;
+        this.flywheels = flywheels;
     }
 
     @Override
     public void periodic() {
         switch (currentState) {
-            case IDLE -> {
-                arm.setSetpoint(Units.degreesToRadians(armIdleSetpointDegrees.get()));
-                shooter.requestRPM(idleLeftRPM.get(), idleRightRPM.get());
-            }
-            case INTAKE -> {
-                arm.setSetpoint(Units.degreesToRadians(armIntakeSetpointDegrees.get()));
-                if (arm.atSetpoint()) {
-                    shooter.requestIntake();
-                }
-            }
-            case PREPARE_SHOOT -> {
-                var aimingParameters = RobotState.getInstance().getAimingParameters();
-                double y = (FieldConstants.Speaker.centerSpeakerOpening.getY()) / 2.0;
-                y += Units.inchesToMeters(yCompensation.get());
-                double x = aimingParameters.effectiveDistance();
-                arm.setSetpoint(Math.atan(y / x));
-                shooter.requestRPM(shootingLeftRPM.get(), shootingRightRPM.get());
-            }
-            case SHOOT -> {
-                if (!atShootingSetpoint()) {
-                    currentState = State.PREPARE_SHOOT;
-                } else {
-                    shooter.requestFeed();
-                    followThroughArmAngle = arm.getAngle().getRadians();
-                    followThroughTimer.restart();
-                }
-            }
+            case IDLE -> {}
+            case INTAKE -> {}
+            case PREPARE_SHOOT -> {}
+            case SHOOT -> {}
         }
 
         Logger.recordOutput("DevBotSuperstructure/currentState", currentState.toString());
@@ -88,7 +62,7 @@ public void periodic() {
     public boolean atShootingSetpoint() {
         return (currentState == State.PREPARE_SHOOT || currentState == State.SHOOT)
                 && arm.atSetpoint()
-                && shooter.atSetpoint();
+                && flywheels.atSetpoint();
     }
 
     public void setDesiredState(State desiredState) {

diff --git a/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java
@@ -7,45 +7,34 @@
 
 public class SuperstructureConstants {
 
-    public static class ShooterConstants {
-        // encoder / flywheelReduction = flywheel
-        public static double flywheelReduction = (1.0 / 2.0);
-        public static double shooterToleranceRPM = 50.0;
+    public static class FeederConstants {
+        public static double reduction = (1.0 / 1.0);
 
-        public static FlywheelConstants leftFlywheelConstants =
+        public static int id =
                 switch (Constants.getRobot()) {
-                    default -> new FlywheelConstants(
-                            5,
-                            false,
-                            0.0,
-                            0.0,
-                            0.0,
-                            0.33329,
-                            0.00083,
-                            0.0);
+                    default -> 6;
                 };
-
-        public static FlywheelConstants rightFlywheelConstants =
+        public static boolean inverted =
                 switch (Constants.getRobot()) {
-                    default -> new FlywheelConstants(4, false, 0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0);
+                    default -> false;
                 };
+    }
 
-        public static FeederConstants feederConstants =
+    public static class FlywheelConstants {
+        // encoder / flywheelReduction = flywheel
+        public static double reduction = (1.0 / 2.0);
+        public static double shooterToleranceRPM = 50.0;
+        public static int leftID = 5;
+        public static int rightID = 6;
+
+        public static Gains gains =
                 switch (Constants.getRobot()) {
-                    default -> new FeederConstants(6, false);
+                    case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
+                    case DEVBOT -> new Gains(0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0);
+                    case COMPBOT -> null;
                 };
 
-        public record FlywheelConstants(
-                int id,
-                boolean inverted,
-                double kP,
-                double kI,
-                double kD,
-                double kS,
-                double kV,
-                double kA) {}
-
-        public record FeederConstants(int id, boolean inverted) {}
+        public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
     }
 
     public static class IntakeConstants {
@@ -64,29 +53,20 @@ public static class ArmConstants {
         // reduction is 12:62 18:60 12:65
         public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0);
         public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0);
-        public static Translation2d armOrigin2d =
-                new Translation2d(-Units.inchesToMeters(0), Units.inchesToMeters(11.75));
-
-        public static Rotation2d minAngle = Rotation2d.fromDegrees(0.0);
+        public static Translation2d armOrigin =
+                new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75));
+        public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0);
         public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0);
 
         public static int leaderID = 25;
         public static int followerID = 26;
         public static int armEncoderID = 1;
 
-        public static boolean leaderInverted =
-                switch (Constants.getRobot()) {
-                    case DEVBOT -> false;
-                    case COMPBOT -> false;
-                    case SIMBOT -> false;
-                };
+        public static boolean leaderInverted = false;
+        public static boolean followerInverted = false;
 
-        public static boolean followerInverted =
-                switch (Constants.getRobot()) {
-                    case DEVBOT -> false;
-                    case COMPBOT -> false;
-                    case SIMBOT -> false;
-                };
+        /** The offset of the arm encoder in rotations. */
+        public static double armEncoderOffsetRotations = 0.0;
 
         public static double armLength =
                 switch (Constants.getRobot()) {
@@ -101,10 +81,7 @@ public static class ArmConstants {
                     case COMPBOT -> new ControllerConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
                 };
 
-        public static ProfileConstraints profileConstraints =
-                switch (Constants.getRobot()) {
-                    default -> new ProfileConstraints(2.0 * Math.PI, 10);
-                };
+        public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10);
 
         public record ProfileConstraints(
                 double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {}