cleanup

scripts/map.json

@@ -1 +1 @@
-{"driver":{"b":"shoot","x":"pivot shooter no","y":"reset heading","leftTrigger":"Slow Drive"},"operator":{"rightStick":"Turret"}}
+{"driver":{"y":"reset heading","leftTrigger":"Slow Drive"},"operator":{"a":"feed","x":"sub","y":"zero","rightBumper":"Intake"}}

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

@@ -24,7 +24,6 @@
 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;
@@ -106,17 +105,17 @@ public class RobotContainer {
           new BeamBreakIOAdafruit(SpindexConstants.kSpindexBeamBreakDIO));
   private final Vision vision = new Vision(new VisionIOLimelight());
 
-  private final Superstructure superstructure =
-      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 =
@@ -160,6 +159,9 @@ private void configureBindings() {
         .onTrue(
             shooter.setVelocity(
                 ShooterConstants.kShooterSpeakerRPS, ShooterConstants.kShooterSpeakerRPS));
+    m_operatorController.a("feed").onTrue(spindex.feedNoteToShooter());
+    m_operatorController.x("sub").onTrue(pivotShooter.setSub());
+    m_operatorController.y("zero").onTrue(pivotShooter.setPosition(0));
   }
 
   private void configureAutoChooser() {

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -42,5 +42,5 @@ public final class IntakeConstants {
                   .withStatorCurrentLimitEnable(true)
                   .withStatorCurrentLimit(80));
   public static int flashConfigRetries = 5;
-  public static double kIntakeRedirectVoltage = -2;
+  public static double kIntakeRedirectVoltage = 5;
 }

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

@@ -85,6 +85,11 @@ public Command slamZero() {
         .andThen(this.zero());
   }
 
+  public Command setSub() {
+    return setPosition(
+        PivotShooterConstants.kSubWooferPreset * PivotShooterConstants.kPivotMotorGearing);
+  }
+
   public Command slamAndPID() {
 
     return Commands.sequence(this.setPosition(0), this.slamZero());

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

@@ -12,61 +12,32 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 
 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 kAmpPreset = (4) / 138.33;
-  public static final double kWingNoteCenterPreset = 5.8 / 138.33;
-  public static final double kWingNoteSidePreset =
-      5.5 / 138.33; // old value: 5.7 distance: -1.5 //old ish?: 5.4
-  public static final double kWingNoteFarSidePreset = 0 / 138.33;
-  public static final double kTrussSourceSidePreset = 6.7 / 138.33; // -10.6875
-  public static final double kHalfWingPodiumPreset =
-      6.55 / 138.33; // old value: 6.7 distance: -11.5275
-  public static final double kPodiumLeftPreset = 6.5 / 138.33;
-  public static final double kPodiumRPreset = 6 / 138.33;
 
   public static final int kPivotMotorID = 60;
 
   /* PID */
-  public static final TrapezoidProfile.Constraints kPivotProfileContraints =
-      new TrapezoidProfile.Constraints(16, 16);
-
-  /* Tolerance/threshold */
-  public static final double kPivotPositionToleranceDeg = 0.1; // 5deg for the pivot.
-  public static final double kStallVelocityThreshold = 0.1;
 
   /* Physics/geometry */
   public static final double kPivotMotorGearing = 138.333; // 22 by 1
-  public static final double kPivotLength = 0.2;
-  public static final double kPivotMinAngleDeg = -90;
-  public static final double kPivotMaxAngleDeg = 50;
-  public static final double kPivotStartingAngleDeg = 0;
-  public static final double jKgMetersSquared = 0.1; // for sim
 
   /* Preset */
   public static final double kPivotSlamIntakeVoltage = -5;
   public static final double kPivotSlamShooterVoltage = -2;
 
   /* Misc */
-  public static final int kNumPivotMotors = 1;
   public static final boolean kUseFOC = false;
   public static final boolean kUseMotionMagic = true; // idk
   public static final double updateFrequency = 50.0;
   public static final int flashConfigRetries = 5;
   public static final double kPivotSlamStallCurrent = 50;
 
-  public static final int kSpeakerAprilTagRed = 4;
-  public static final int kSpeakerAprilTagBlue = 0;
-
-  public static final int kSpeakerBackupAprilTagRed = 5;
-  public static final int kSpeakerBackupAprilTagBlue = 1;
-
   public static final TalonFXConfiguration motorConfigs =
       new TalonFXConfiguration()
           .withSlot0(

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

@@ -19,8 +19,10 @@
 public class PivotShooterIOTalonFX implements PivotShooterIO {
 
   private final TalonFX pivotShooterMotor = new TalonFX(PivotShooterConstants.kPivotMotorID);
-  private final PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0);
-  private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0).withSlot(0);
+  private final PositionVoltage positionRequest =
+      new PositionVoltage(0).withSlot(0).withEnableFOC(PivotShooterConstants.kUseFOC);
+  private final MotionMagicVoltage motionMagicRequest =
+      new MotionMagicVoltage(0).withSlot(0).withEnableFOC(PivotShooterConstants.kUseFOC);
   private final VoltageOut voltageReq = new VoltageOut(0);
 
   private final StatusSignal<Double> pivotShooterMotorVoltage = pivotShooterMotor.getMotorVoltage();