temp commit for indexer

NOT FINISHED INDEXER CODE

src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java

@@ -26,35 +26,11 @@
 /** Add your docs here. */
 public class ElevatorIOSim implements ElevatorIO {
 
-<<<<<<< Updated upstream
-  // SIM VARIABLES (CHANGE)
-  private int gearBoxMotorCount = 2;
-  private int gearing = 1;
-  private double carriageMassKg = 1;
-  private double drumRadiusMeters = 0.01;
-  private double minHeightMeters = 0;
-  private double maxHeightMeters = 3;
-  private boolean simulateGravity = true;
-  private double initialPositionMeters = 0.0;
-
-  private final DCMotor simGearbox = DCMotor.getFalcon500(gearBoxMotorCount);
-  private ElevatorSim sim =
-      new ElevatorSim(
-          simGearbox,
-          gearing,
-          carriageMassKg,
-          drumRadiusMeters,
-          minHeightMeters,
-          maxHeightMeters,
-          simulateGravity,
-          initialPositionMeters);
-=======
   private final DCMotor simGearbox = DCMotor.getKrakenX60Foc(2);
   private ElevatorSim sim = new ElevatorSim(simGearbox, 1, 1, 0.01, 0.0, 3, true, 0.0);
->>>>>>> Stashed changes
   private PIDController pid = new PIDController(0, 0, 0);
 
-  private Distance positionInches = Inches.of(0);
+  private Distance position = Inches.of(0);
   private LinearVelocity velocityInchPerSec = InchesPerSecond.of(0);
   private Voltage appliedVolts = Volts.of(0.0);
   private Current currentAmps = Amps.of(0.0);
@@ -69,33 +45,21 @@ public class ElevatorIOSim implements ElevatorIO {
   public void updateInputs(ElevatorIOInputs inputs) {
     positionSetpointInches = Inches.of(pid.getSetpoint());
 
-<<<<<<< Updated upstream
-    appliedVolts +=
-        MathUtil.clamp(
-            pid.calculate(sim.getPositionMeters() * metersToInches, positionSetpointInches),
-            clampedValueLowVolts,
-            clampedValueHighVolts);
-=======
     appliedVolts.plus(
         Volts.of(MathUtil.clamp(
             pid.calculate(Meters.of(sim.getPositionMeters()).in(Inches), positionSetpointInches.in(Inches)), -12.0, 12)));
->>>>>>> Stashed changes
 
     sim.setInputVoltage(appliedVolts.in(Volts));
 
-<<<<<<< Updated upstream
-    positionInches = sim.getPositionMeters() * metersToInches;
-    velocityInchPerSec = sim.getVelocityMetersPerSecond() * metersToInches;
-    currentAmps = sim.getCurrentDrawAmps();
-=======
-    positionInches = Inches.of(Meters.of(sim.getPositionMeters()).in(Inches));
+    position = Inches.of(Meters.of(sim.getPositionMeters()).in(Inches));
+    position = Meters.of(sim.getPositionMeters());
+
     velocityInchPerSec = InchesPerSecond.of(MetersPerSecond.of(sim.getVelocityMetersPerSecond()).in(MetersPerSecond));
     currentAmps = Amps.of(sim.getCurrentDrawAmps());
->>>>>>> Stashed changes
 
-    inputs.positionSetpoint = positionSetpointInches;
+    inputs.positionSetpointInch = position.plus(Meters.of(2)); 
     inputs.appliedVolts = appliedVolts.in(Volts);
-    inputs.elevatorPosition = positionInches;
+    inputs.elevatorPosition = position;
     inputs.elevatorVelocity = velocityInchPerSec;
     inputs.currentAmps = currentAmps.in(Amps);
 
@@ -108,20 +72,15 @@ public void runCharacterization(double volts) {
   }
 
   @Override
-  public void setPositionSetpoint(double positionInches, Voltage ffVolts) {
+  public void setPositionSetpoint(double position, Voltage ffVolts) {
     appliedVolts = ffVolts;
-    pid.setSetpoint(positionInches);
+    pid.setSetpoint(position);
   }
 
   @Override
   public void stop() {
-<<<<<<< Updated upstream
-    appliedVolts = 0;
-    pid.setSetpoint(sim.getPositionMeters() * metersToInches);
-=======
     appliedVolts = Volts.of(0);
     pid.setSetpoint(sim.getPositionMeters() * 39.37);
->>>>>>> Stashed changes
   }
 
   @Override

src/main/java/frc/robot/subsystems/indexers/Indexer.java

@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems.indexers;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.PhysicalConstants;
 
@@ -13,23 +15,65 @@ public class Indexer extends SubsystemBase {
 
   private final IndexerIOInputsAutoLogged Iinputs = new IndexerIOInputsAutoLogged();
 
+  private static double kP;
+  private static double kG;
+  private static double kV;
+
+  private static double maxVelocityRotPerSec;
+  private static double maxAccelerationRotPerSecSquared;
+
+  private TrapezoidProfile indexerProfile;
+  private TrapezoidProfile.Constraints indexerConstraints;
+
+  private TrapezoidProfile.State indexerGoalStateRotations = new TrapezoidProfile.State();
+  private TrapezoidProfile.State indexerCurrentStateRotations = new TrapezoidProfile.State();
+
   public Indexer(IndexerIO indexer) {
     this.indexer = indexer;
 
-    switch (PhysicalConstants.currentMode) {
+    switch (PhysicalConstants.getMode()) {
       case REAL:
+        kG = 0.29;
+        kV = 1;
+        kP = 1.123;
+        break;
       case REPLAY:
+        kG = 0.29;
+        kV = 1;
+        kP = 1.123;
         break;
       case SIM:
+        kG = 0.29;
+        kV = 1;
+        kP = 1.123;
         break;
       default:
+        kG = 0.29;
+        kV = 1;
+        kP = 1.123;
         break;
     }
+
+    maxVelocityRotPerSec = 4;
+    maxAccelerationRotPerSecSquared = 4;
+
+    indexerConstraints = new TrapezoidProfile.Constraints(maxVelocityRotPerSec, maxAccelerationRotPerSecSquared);
+    indexerProfile = new TrapezoidProfile(indexerConstraints);
+
+    indexerCurrentStateRotations = indexerProfile.calculate(0, indexerCurrentStateRotations, indexerGoalStateRotations);
+
+    indexer.configurePID(kP, 0, 0);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
     indexer.updateInputs(Iinputs);
   }
+
+  public void index(double linearDistanceInches) {
+
+    indexer.setPosition(positionRots, ffvolts);
+  }
+
 }