add back superstruc

scripts/map.json

@@ -1 +1 @@
-{"driver":{"y":"reset heading","leftTrigger":"Slow Drive"},"operator":{"a":"feed","x":"sub","y":"zero","rightBumper":"Intake"}}
+{"driver":{"a":"Intake","y":"reset heading","leftTrigger":"Slow Drive"},"operator":{}}

simgui-ds.json

@@ -1,4 +1,14 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
+  "Keyboard 1 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -92,6 +102,9 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "Keyboard1"
     }
   ]
 }

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

@@ -27,6 +27,7 @@
 import frc.robot.commands.AutoRoutines;
 import frc.robot.sim.SimMechs;
 import frc.robot.subsystems.BeamBreakIOAdafruit;
+import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.ampevator.Ampevator;
 import frc.robot.subsystems.ampevator.AmpevatorIOSim;
 import frc.robot.subsystems.ampevator.AmpevatorIOTalonFX;
@@ -109,18 +110,17 @@ public class RobotContainer {
           new BeamBreakIOAdafruit(SpindexConstants.kSpindexBeamBreakDIO));
   private final Vision vision = new Vision(new VisionIOLimelight());
 
-  // private final Superstructure superstructure = // TODO: when uncommented also uncomment the
-  // periodic
-  // new Superstructure(
-  // ampevator,
-  // ampevatorRollers,
-  // turret,
-  // climb,
-  // intake,
-  // spindex,
-  // pivotShooter,
-  // shooter,
-  // vision);
+  private final Superstructure superstructure =
+      new Superstructure(
+          ampevator,
+          ampevatorRollers,
+          turret,
+          climb,
+          intake,
+          spindex,
+          pivotShooter,
+          shooter,
+          vision);
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
   public final MappedXboxController m_driverController =
@@ -157,21 +157,14 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
-    m_operatorController
-        .rightBumper("Intake")
-        .whileTrue(
-            intake
-                .intakeIn()
-                .alongWith(spindex.goToShooter())
-                .alongWith(turret.setPosition(TurretConstants.kIntakePreset)));
+    m_driverController
+        .a("Intake")
+        .onTrue(superstructure.setState(Superstructure.StructureState.INTAKE));
     m_operatorController
         .rightTrigger("Shooter")
         .onTrue(
             shooter.setVelocity(
                 ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterFollowerSpeakerRPS));
-    m_operatorController.a("feed").onTrue(spindex.feedNoteToShooter());
-    m_operatorController.x("sub").onTrue(pivotShooter.setSub());
-    m_operatorController.y("zero").onTrue(pivotShooter.setPosition(0));
   }
 
   private void configureRumble() {
@@ -425,6 +418,6 @@ public Command getAutonomousCommand() {
 
   public void periodic() {
     autoChooser.update();
-    // superstructure.periodic();
+    superstructure.periodic();
   }
 }

src/main/java/frc/robot/subsystems/Superstructure.java

@@ -40,7 +40,8 @@ public static enum StructureState {
     FEEDING,
     TRAP_PREP,
     TRAPPING,
-    POST_TRAP
+    POST_TRAP,
+    INTAKE
   }
 
   private final Ampevator ampevator;
@@ -125,10 +126,7 @@ public void configStateTransitions() {
         .onTrue(setState(StructureState.HOME));
     stateTriggers
         .get(StructureState.SUB_PREP)
-        .onTrue(spindex.goToShooter())
-        .onTrue(
-            pivotShooter.setPosition(
-                PivotShooterConstants.kSubWooferPreset * PivotShooterConstants.kPivotMotorGearing))
+        .onTrue(pivotShooter.setPosition(PivotShooterConstants.kSubWooferPreset))
         .onTrue(
             shooter.setVelocity(
                 ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterFollowerSpeakerRPS))
@@ -141,6 +139,13 @@ public void configStateTransitions() {
         .onTrue(spindex.feedNoteToShooter())
         .and(spindex.debouncedBeamBreak.debounce(1))
         .onTrue(setState(StructureState.HOME));
+    stateTriggers
+        .get(StructureState.INTAKE)
+        .onTrue(intake.intakeIn())
+        .onTrue(spindex.goToShooter())
+        .onTrue(turret.setPosition(TurretConstants.kIntakePreset))
+        .and(spindex.debouncedBeamBreak.debounce(0.5))
+        .onTrue(setState(StructureState.IDLE));
   }
 
   // call manually

src/main/java/frc/robot/subsystems/pivotshooter/PivotShooter.java

@@ -75,11 +75,6 @@ public Command off() {
     return this.runOnce(pivotShooterIO::off);
   }
 
-  public Command setSub() {
-    return setPosition(
-        PivotShooterConstants.kSubWooferPreset * PivotShooterConstants.kPivotMotorGearing);
-  }
-
   public Command zero() {
     return this.runOnce(pivotShooterIO::zero);
   }
@@ -89,9 +84,8 @@ public Command bruh(Vision vision) {
         () -> {
           pivotShooterIO.setPosition(
               aprilTagMap.get(
-                      (vision.getLastCenterLimelightY() - vision.getLastLastCenterLimelightY())
-                          + vision.getCenterLimelightY())
-                  * PivotShooterConstants.kPivotMotorGearing);
+                  (vision.getLastCenterLimelightY() - vision.getLastLastCenterLimelightY())
+                      + vision.getCenterLimelightY()));
         });
   }
 

src/main/java/frc/robot/subsystems/pivotshooter/PivotShooterConstants.java

@@ -17,14 +17,11 @@
 public final class PivotShooterConstants {
   // public static final double kSubWooferPreset = (3.5 + 0.3) / 138.33; // idk if
   // this works
-  public static final double kSubWooferPreset = 3.4 / 138.33; // 3.2
-  public static final double kFeederPreset = 5.9 / 138.33;
+  public static final double kSubWooferPreset = 3.4; // 3.2
+  public static final double kFeederPreset = 5.9;
 
   public static final int kPivotMotorID = 51;
 
-  /* Physics/geometry */
-  public static final double kPivotMotorGearing = 138.333; // TODO: get from nate
-
   // max value is 8, min is 0
 
   /* Misc */

src/main/java/frc/robot/subsystems/pivotshooter/PivotShooterIO.java

@@ -17,11 +17,9 @@ public static class PivotShooterIOInputs {
     public double pivotShooterMotorVoltage = 0.0;
     public double pivotShooterMotorVelocity = 0.0;
     public double pivotShooterMotorPosition = 0.0;
-    public double pivotShooterMotorDegrees = 0.0;
     public double pivotShooterMotorStatorCurrent = 0.0;
     public double pivotShooterMotorSupplyCurrent = 0.0;
     public double pivotShooterMotorTemperature = 0.0;
-    public double pivotShooterMotorReferenceSlope = 0.0;
   }
 
   public default void updateInputs(PivotShooterIOInputs inputs) {}

src/main/java/frc/robot/subsystems/pivotshooter/PivotShooterIOTalonFX.java

@@ -34,8 +34,6 @@ public class PivotShooterIOTalonFX implements PivotShooterIO {
       pivotShooterMotor.getSupplyCurrent();
   private final StatusSignal<Double> pivotShooterMotorTemperature =
       pivotShooterMotor.getDeviceTemp();
-  private final StatusSignal<Double> pivotShooterMotorReferenceSlope =
-      pivotShooterMotor.getClosedLoopReferenceSlope();
 
   public PivotShooterIOTalonFX() {
     PhoenixUtil.applyMotorConfigs(
@@ -50,8 +48,7 @@ public PivotShooterIOTalonFX() {
         pivotShooterMotorPosition,
         pivotShooterMotorStatorCurrent,
         pivotShooterMotorSupplyCurrent,
-        pivotShooterMotorTemperature,
-        pivotShooterMotorReferenceSlope);
+        pivotShooterMotorTemperature);
     pivotShooterMotor.optimizeBusUtilization();
   }
 
@@ -63,17 +60,13 @@ public void updateInputs(PivotShooterIOInputs inputs) {
         pivotShooterMotorPosition,
         pivotShooterMotorStatorCurrent,
         pivotShooterMotorSupplyCurrent,
-        pivotShooterMotorTemperature,
-        pivotShooterMotorReferenceSlope);
+        pivotShooterMotorTemperature);
     inputs.pivotShooterMotorVoltage = pivotShooterMotorVoltage.getValueAsDouble();
     inputs.pivotShooterMotorVelocity = pivotShooterMotorVelocity.getValueAsDouble();
     inputs.pivotShooterMotorPosition = pivotShooterMotorPosition.getValueAsDouble();
-    inputs.pivotShooterMotorDegrees =
-        (inputs.pivotShooterMotorPosition / PivotShooterConstants.kPivotMotorGearing) * 360;
     inputs.pivotShooterMotorStatorCurrent = pivotShooterMotorStatorCurrent.getValueAsDouble();
     inputs.pivotShooterMotorSupplyCurrent = pivotShooterMotorSupplyCurrent.getValueAsDouble();
     inputs.pivotShooterMotorTemperature = pivotShooterMotorTemperature.getValueAsDouble();
-    inputs.pivotShooterMotorReferenceSlope = pivotShooterMotorReferenceSlope.getValueAsDouble();
   }
 
   @Override

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -57,7 +57,8 @@ public final class ShooterConstants {
       motorConfigs.withMotorOutput(
           motorOutputConfigs.withInverted(InvertedValue.Clockwise_Positive));
 
-  public static double kShooterSpeakerRPS = -30;
+  public static double kShooterSpeakerRPS =
+      -30; // i got tired of trying to reverse the shooter wheels
   public static double kShooterFollowerSpeakerRPS = 60; // really 80
 
   public static double kShooterSubwooferRPS = -60;