The Bad Pivot Code I forgot to Commit

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

@@ -13,8 +13,12 @@
 
 package frc.robot;
 
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -34,8 +38,8 @@
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOSim;
 import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import frc.robot.subsystems.pivot.Pivot;
+import frc.robot.subsystems.pivot.PivotIOSim;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -47,6 +51,7 @@ public class RobotContainer {
   // Subsystems
   private final Drive drive;
   private final Flywheel flywheel;
+  private final Pivot pivot;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -69,6 +74,8 @@ public RobotContainer() {
                 new ModuleIOSparkMax(2),
                 new ModuleIOSparkMax(3));
         flywheel = new Flywheel(new FlywheelIOSparkMax());
+        //change perchance
+        pivot = Pivot.initialize(new PivotIOSim());
         // drive = new Drive(
         // new GyroIOPigeon2(),
         // new ModuleIOTalonFX(0),
@@ -88,6 +95,7 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         flywheel = new Flywheel(new FlywheelIOSim());
+        pivot = Pivot.initialize(new PivotIOSim());
         break;
 
       default:
@@ -100,6 +108,8 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {});
         flywheel = new Flywheel(new FlywheelIO() {});
+        //change?
+        pivot = Pivot.initialize(new PivotIOSim());
         break;
     }
 
@@ -165,6 +175,11 @@ private void configureButtonBindings() {
         .whileTrue(
             Commands.startEnd(
                 () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+    controller
+        .y()
+        .whileTrue(
+            Commands.startEnd(
+                () -> pivot.setVelocity(, pivot::stop, pivot)));
   }
 
   /**

src/main/java/frc/robot/subsystems/pivot/Pivot.java

@@ -0,0 +1,65 @@
+package frc.robot.subsystems.pivot;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.pivot.PivotIO;
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import frc.robot.Constants;
+
+public class Pivot extends SubsystemBase {
+
+    private static Pivot pivotInstance = null;
+    public final PivotIO io;
+    private final SimpleMotorFeedforward ffModel;
+    private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged();
+
+    private Pivot(PivotIO io) {
+
+        this.io = io;
+
+        switch (Constants.currentMode) {
+            case REAL:
+            case REPLAY:
+              ffModel = new SimpleMotorFeedforward(0.0, 0.0);
+              io.configurePID(1.0, 0.0, 0.0);
+              break;
+            case SIM:
+              ffModel = new SimpleMotorFeedforward(0.0, 0.0);
+              io.configurePID(1.0, 0.0, 0.0);
+              break;
+            default:
+              ffModel = new SimpleMotorFeedforward(0.0, 0.0);
+              break;
+        }
+
+
+    }
+
+    public static Pivot getInstance() {
+        return pivotInstance;
+    }
+
+    public static Pivot initialize(PivotIO io) {
+        if (Pivot.pivotInstance == null) {
+            pivotInstance = new Pivot(io);
+        }
+        return pivotInstance;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    Logger.processInputs("Pivot", inputs);
+    }
+
+    public void runVoltage(double volts) {
+        io.setVoltage(volts);
+    }
+
+    public void stop() {
+        io.stop();
+    }
+
+
+}

src/main/java/frc/robot/subsystems/pivot/PivotIO.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.pivot;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface PivotIO {
+
+    @AutoLog
+  public static class PivotIOInputs {
+    public double positionRad = 0.0;
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+    public double[] currentAmps = new double[] {};
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(PivotIOInputs inputs) {}
+
+  /** Run open loop at the specified voltage. */
+  public default void setVoltage(double volts) {}
+
+  /** Run closed loop at the specified velocity. */
+  public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
+
+  /** Stop in open loop. */
+  public default void stop() {}
+
+  /** Set velocity PID constants. */
+  public default void configurePID(double kP, double kI, double kD) {}
+
+}

src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java

@@ -0,0 +1,38 @@
+// package frc.robot.subsystems.pivot;
+
+// import edu.wpi.first.math.MathUtil;
+// import edu.wpi.first.math.controller.PIDController;
+// import edu.wpi.first.math.system.plant.DCMotor;
+// import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+// import frc.robot.subsystems.flywheel.FlywheelIO.FlywheelIOInputs;
+
+// public class PivotIOReal implements PivotIO {
+
+
+
+
+//     @Override
+//   public void setVoltage(double volts) {
+//     closedLoop = false;
+//     appliedVolts = volts;
+//     sim.setInputVoltage(volts);
+//   }
+
+//   @Override
+//   public void setVelocity(double velocityRadPerSec, double ffVolts) {
+//     closedLoop = true;
+//     pid.setSetpoint(velocityRadPerSec);
+//     this.ffVolts = ffVolts;
+//   }
+
+//   @Override
+//   public void stop() {
+//     setVoltage(0.0);
+//   }
+
+//   @Override
+//   public void configurePID(double kP, double kI, double kD) {
+//     pid.setPID(kP, kI, kD);
+//   }
+
+// }

src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.pivot;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.subsystems.flywheel.FlywheelIO.FlywheelIOInputs;
+
+public class PivotIOSim implements PivotIO {
+    private SingleJointedArmSim sim = new SingleJointedArmSim(DCMotor.getNEO(1), 1.5, 
+    0.004, 0, 0, 0, 
+    false, 0);
+    private PIDController pid = new PIDController(1.0, 0.0, 0.0);
+
+    private boolean closedLoop = false;
+    private double ffVolts = 0.0;
+    private double appliedVolts = 0.0;
+
+      @Override
+    public void updateInputs(PivotIOInputs inputs) {
+        if (closedLoop) {
+        appliedVolts =
+            MathUtil.clamp(pid.calculate(sim.getVelocityRadPerSec()) + ffVolts, -12.0, 12.0);
+            sim.setInputVoltage(appliedVolts);
+        }
+    }
+
+    @Override
+    public void rotatePivot(double speed) {
+        pivotAppliedVolts = MathUtil.clamp(12 * speed, -12, 12);
+        sim.setInputVoltage(appliedVolts);
+    }
+
+    @Override
+    public void setVoltage(double volts) {
+        sim.setInputVoltage(volts);
+    }
+
+
+}