Add Speaker to constants

Finish devbot superstructure
Mechanical-Advantage · Feb 10, 2024 · be5f093 · be5f093
1 parent ba62b3b
commit be5f093
Show file tree

Hide file tree

Showing 6 changed files with 30 additions and 29 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java
@@ -27,7 +27,7 @@
  */
 public final class Constants {
     public static final int loopPeriodMs = 20;
-    private static RobotType robotType = RobotType.DEVBOT;
+    private static RobotType robotType = RobotType.SIMBOT;
     public static final boolean tuningMode = true;
 
     private static boolean invalidRobotAlertSent = false;

diff --git a/...in/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java b/...in/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java
@@ -47,7 +47,6 @@ public enum SystemState {
     private final Feeder feeder;
 
     private final Timer followThroughTimer = new Timer();
-    private double followThroughArmAngle = 0.0;
 
     public DevBotSuperstructure(Arm arm, Flywheels flywheels, Feeder feeder) {
         this.arm = arm;
@@ -79,6 +78,7 @@ public void periodic() {
                 } else if (atShootingSetpoint()) {
                     currentState = SystemState.SHOOT;
                     followThroughTimer.restart();
+                    goalState = SystemState.IDLE;
                 }
             }
         }
@@ -102,6 +102,7 @@ public void periodic() {
                                 FieldConstants.Speaker.centerSpeakerOpening.getZ()
                                         + Units.inchesToMeters(yCompensation.get())));
                 flywheels.setSetpointRpm(shootingLeftRPM.get(), shootingRightRPM.get());
+                feeder.runVolts(0.0);
             }
             case SHOOT -> {
                 feeder.runVolts(2.0);
@@ -114,6 +115,6 @@ public void periodic() {
 
     @AutoLogOutput(key = "DevBotSuperstructure/ReadyToShoot")
     public boolean atShootingSetpoint() {
-        return arm.atSetpoint() && flywheels.atSetpoint();
+        return flywheels.atSetpoint();
     }
 }
diff --git a/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/...java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java
@@ -75,19 +75,19 @@ public static class ArmConstants {
                     default -> Units.inchesToMeters(25.866);
                 };
 
-        public static ControllerConstants controllerConstants =
+        public static Gains gains =
                 switch (Constants.getRobot()) {
-                    case SIMBOT -> new ControllerConstants(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01);
-                    case DEVBOT -> new ControllerConstants(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12);
-                    case COMPBOT -> new ControllerConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+                    case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01);
+                    case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12);
+                    case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
                 };
 
         public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10);
 
         public record ProfileConstraints(
                 double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {}
 
-        public record ControllerConstants(
+        public record Gains(
                 double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {}
     }
 }
diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -20,19 +20,19 @@
 
 public class Arm extends SubsystemBase {
     private static final LoggedTunableNumber kP =
-            new LoggedTunableNumber("Arm/kP", controllerConstants.kP());
+            new LoggedTunableNumber("Arm/kP", gains.kP());
     private static final LoggedTunableNumber kI =
-            new LoggedTunableNumber("Arm/kI", controllerConstants.kI());
+            new LoggedTunableNumber("Arm/kI", gains.kI());
     private static final LoggedTunableNumber kD =
-            new LoggedTunableNumber("Arm/kD", controllerConstants.kD());
+            new LoggedTunableNumber("Arm/kD", gains.kD());
     private static final LoggedTunableNumber kS =
-            new LoggedTunableNumber("Arm/kS", controllerConstants.ffkS());
+            new LoggedTunableNumber("Arm/kS", gains.ffkS());
     private static final LoggedTunableNumber kV =
-            new LoggedTunableNumber("Arm/kV", controllerConstants.ffkV());
+            new LoggedTunableNumber("Arm/kV", gains.ffkV());
     private static final LoggedTunableNumber kA =
-            new LoggedTunableNumber("Arm/kA", controllerConstants.ffkA());
+            new LoggedTunableNumber("Arm/kA", gains.ffkA());
     private static final LoggedTunableNumber kG =
-            new LoggedTunableNumber("Arm/kG", controllerConstants.ffkG());
+            new LoggedTunableNumber("Arm/kG", gains.ffkG());
     private static final LoggedTunableNumber armVelocity =
             new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec());
     private static final LoggedTunableNumber armAcceleration =

diff --git a/...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/...main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java
@@ -61,13 +61,13 @@ public ArmIOKrakenFOC() {
 
         controllerConfig =
                 new Slot0Configs()
-                        .withKP(controllerConstants.kP())
-                        .withKI(controllerConstants.kI())
-                        .withKD(controllerConstants.kD())
-                        .withKS(controllerConstants.ffkS())
-                        .withKV(controllerConstants.ffkV())
-                        .withKA(controllerConstants.ffkA())
-                        .withKG(controllerConstants.ffkG())
+                        .withKP(gains.kP())
+                        .withKI(gains.kI())
+                        .withKD(gains.kD())
+                        .withKS(gains.ffkS())
+                        .withKV(gains.ffkV())
+                        .withKA(gains.ffkA())
+                        .withKG(gains.ffkG())
                         .withGravityType(GravityTypeValue.Arm_Cosine);
         leaderConfig.Slot0 = controllerConfig;
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java
@@ -34,15 +34,15 @@ public class ArmIOSim implements ArmIO {
     public ArmIOSim() {
         ff =
                 new ArmFeedforward(
-                        controllerConstants.ffkS(),
-                        controllerConstants.ffkG(),
-                        controllerConstants.ffkV(),
-                        controllerConstants.ffkA());
+                        gains.ffkS(),
+                        gains.ffkG(),
+                        gains.ffkV(),
+                        gains.ffkA());
         profiledController =
                 new ProfiledPIDController(
-                        controllerConstants.kP(),
-                        controllerConstants.kI(),
-                        controllerConstants.kD(),
+                        gains.kP(),
+                        gains.kI(),
+                        gains.kD(),
                         new TrapezoidProfile.Constraints(
                                 profileConstraints.cruiseVelocityRadPerSec(),
                                 profileConstraints.accelerationRadPerSec2()),