More state machine updates

Mechanical-Advantage · Feb 10, 2024 · 1772db5 · 1772db5
1 parent 1655e5d
commit 1772db5
Show file tree

Hide file tree

Showing 4 changed files with 53 additions and 21 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -41,12 +41,14 @@
 import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder;
 import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIO;
 import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSim;
+import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSparkFlex;
 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.IntakeIOKrakenFOC;
 import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOSim;
 import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
 import org.littletonrobotics.frc2024.util.trajectory.ChoreoTrajectoryDeserializer;
@@ -94,7 +96,7 @@ public RobotContainer() {
                                     new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]));
                     arm = new Arm(new ArmIOKrakenFOC());
                     flywheels = new Flywheels(new FlywheelsIOSparkFlex());
-                    feeder = new Feeder(new FeederIOSim());
+                    feeder = new Feeder(new FeederIOSparkFlex());
                     aprilTagVision =
                             new AprilTagVision(
                                     new AprilTagVisionIONorthstar(

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -81,8 +81,8 @@ public void periodic() {
         switch (currentState) {
             case IDLE -> {
                 arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get()));
-                flywheels.runVolts(4.0, 4.0);
-                feeder.stop();
+                flywheels.setGoal(Flywheels.Goal.IDLE);
+                feeder.setGoal(Feeder.Goal.IDLE);
             }
             case INTAKE -> {
                 arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get()));

diff --git a/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java
@@ -25,7 +25,7 @@ public static class FlywheelConstants {
         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 int rightID = 4;
 
         public static Gains gains =
                 switch (Constants.getRobot()) {

diff --git a/...ain/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/...ain/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java
@@ -4,6 +4,8 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import lombok.Getter;
+import lombok.Setter;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -15,6 +17,20 @@ public class Flywheels extends SubsystemBase {
     private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS());
     private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV());
     private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA());
+
+    private static LoggedTunableNumber shootingLeftRPM =
+            new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0);
+    private static LoggedTunableNumber shootingRightRPM =
+            new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0);
+    private static LoggedTunableNumber idleLeftRPM =
+            new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0);
+    private static LoggedTunableNumber idleRightRPM =
+            new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0);
+
+    private static LoggedTunableNumber intakingLeftRPM =
+            new LoggedTunableNumber("Superstructure/IdleLeftRPM", 2000.0);
+    private static LoggedTunableNumber intakingRightRPM =
+            new LoggedTunableNumber("Superstructure/IdleRightRPM", -2000.0);
     private static final LoggedTunableNumber shooterTolerance =
             new LoggedTunableNumber("Flywheels/ToleranceRPM", shooterToleranceRPM);
 
@@ -24,6 +40,28 @@ public class Flywheels extends SubsystemBase {
     private Double leftSetpointRpm = null;
     private Double rightSetpointRPM = null;
 
+    public enum Goal {
+        IDLE,
+        SHOOTING,
+        INTAKING,
+
+        CHARACTERIZATION
+    }
+
+    @Getter @Setter private Goal goal = Goal.IDLE;
+
+    private void setSetpoint(double left, double right) {
+        leftSetpointRpm = left;
+        rightSetpointRPM = right;
+        io.runVelocity(left, right);
+    }
+
+    private void stop() {
+        leftSetpointRpm = null;
+        rightSetpointRPM = null;
+        io.stop();
+    }
+
     public Flywheels(FlywheelsIO io) {
         System.out.println("[Init] Creating Shooter");
         this.io = io;
@@ -40,14 +78,17 @@ public void periodic() {
         Logger.processInputs("Flywheels", inputs);
 
         if (DriverStation.isDisabled()) {
-            io.stop();
-            leftSetpointRpm = null;
-            rightSetpointRPM = null;
-        } else if (leftSetpointRpm != null && rightSetpointRPM != null) {
-            // Run to setpoint
-            io.runVelocity(leftSetpointRpm, rightSetpointRPM);
+            stop();
+            setGoal(Goal.IDLE);
+        } else {
+            switch (goal) {
+                case IDLE -> setSetpoint(idleLeftRPM.get(), idleRightRPM.get());
+                case INTAKING -> setSetpoint(intakingLeftRPM.get(), intakingRightRPM.get());
+                case SHOOTING -> setSetpoint(shootingLeftRPM.get(), shootingRightRPM.get());
+            }
         }
 
+        Logger.recordOutput("Flywheels/Goal", goal);
         Logger.recordOutput(
                 "Flywheels/LeftSetpointRPM", leftSetpointRpm != null ? leftSetpointRpm : 0.0);
         Logger.recordOutput(
@@ -56,17 +97,6 @@ public void periodic() {
         Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm);
     }
 
-    public void setSetpointRpm(double leftRpm, double rightRpm) {
-        leftSetpointRpm = leftRpm;
-        rightSetpointRPM = rightRpm;
-    }
-
-    public void runVolts(double leftVolts, double rightVolts) {
-        leftSetpointRpm = null;
-        rightSetpointRPM = null;
-        io.runVolts(leftVolts, rightVolts);
-    }
-
     public void runLeftCharacterizationVolts(double volts) {
         io.runCharacterizationLeftVolts(volts);
     }