George here is your s**t

simgui-ds.json

@@ -34,7 +34,11 @@
         88,
         67,
         86,
-        -1
+        80,
+        73,
+        85,
+        89,
+        84
       ],
       "povConfig": [
         {
@@ -57,7 +61,6 @@
           "incKey": 76
         },
         {
-          "decKey": 73,
           "incKey": 75
         }
       ],

src/main/java/frc/robot/OperatorInput.java

@@ -17,7 +17,7 @@ public class OperatorInput {
   public final Trigger slowModeHold = driver.leftTrigger();
   public final Trigger turboModeHold = driver.rightTrigger();
 
-  final Trigger resetEncoder = driver.start();
+  final Trigger resetEncoder = operatorControl.a();
 
   final Trigger elevatorsetpoint1 = operatorControl.x();
   final Trigger elevatorsetpoint2 = operatorControl.y();

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

@@ -192,7 +192,7 @@ void loadCommands() {
     operatorInput.elevatorsetpoint2.onTrue(new ElevatorSetPointCommand(elevatorSubsystem, ElevatorConstants.L3));
     operatorInput.elevatorsetpoint3.onTrue(new ElevatorSetPointCommand(elevatorSubsystem, ElevatorConstants.L4));
 
-    operatorInput.resetEncoder.onTrue(new ResetEncoderCommand(elevatorSubsystem));
+    operatorInput.resetEncoder.onTrue(new ResetEncoderCommand(elevatorSubsystem, ElevatorIO.ElevatorIOInputs));
 
     operatorInput.movementDesired.whileTrue(
         DriveCommands.BaseDriveCommand(

src/main/java/frc/robot/commands/ElevatorSetPointCommand.java

@@ -9,21 +9,28 @@
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorIOSparkMax;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIO;
-
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
 public class ElevatorSetPointCommand extends Command {
     @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
     // The subsystem the command runs on
-    private final ElevatorSubsystem m_elevator;
+    public ElevatorSubsystem m_elevator;
 
     public ElevatorSetPointCommand(ElevatorSubsystem elevator, double setpoint) {
         m_elevator = elevator;
         addRequirements(m_elevator);
-        elevator.profileGoal = new TrapezoidProfile.State(setpoint, 0);
-        elevator.profileSetPoint = elevator.elevatorProfile.calculate(ElevatorConstants.kDt, elevator.profileSetPoint, elevator.profileGoal);
+        m_elevator.profileGoal = new TrapezoidProfile.State(setpoint, 0);
+        m_elevator.profileSetPoint = m_elevator.elevatorProfile.calculate(ElevatorConstants.kDt, elevator.profileSetPoint, elevator.profileGoal);
     }
 
-    public void execute(ElevatorSubsystem elevator){
-        this.m_elevator.moveElevatorToVelocity(1.0, elevator.profileSetPoint.position);
+    @Override
+    public void execute(){
+        this.m_elevator.moveElevatorToVelocity(1.0, m_elevator.profileSetPoint.position);
+        this.m_elevator.getTotalOutput(1.0,m_elevator.profileSetPoint.position);
+        Logger.recordOutput("ElevatorSubsystem/velocity", m_elevator.profileSetPoint.velocity);
+        Logger.recordOutput("ElevatorSubsystem/position", m_elevator.profileSetPoint.position);
+        Logger.recordOutput("ElevatorSubsystem/TotalVoltage", m_elevator.getTotalOutput(1.0,m_elevator.profileSetPoint.position));
+
     }
 
     public boolean isFinished() {

src/main/java/frc/robot/commands/ResetEncoderCommand.java

@@ -1,14 +1,21 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ElevatorConstants;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIO;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorIO;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
+import org.littletonrobotics.junction.Logger;
 
 public class ResetEncoderCommand  extends Command implements ElevatorEncoderIO {
-    public ElevatorSubsystem elevator;
+    public ElevatorSubsystem m_elevator;
     // Constructor with parameters
-    public ResetEncoderCommand(ElevatorSubsystem elevator) {
-        this.elevator = elevator;
+    public ResetEncoderCommand(ElevatorSubsystem elevator, ElevatorIO.ElevatorIOInputs inputs) {
+        this.m_elevator = elevator;
+        addRequirements(m_elevator);
+        inputs.loadHeight = 0;
+        Logger.recordOutput("ElevatorSubsystem/position", inputs.loadHeight);
     }
 
     public void execute(){

src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorIO.java

@@ -5,6 +5,8 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ElevatorIO {
+    ElevatorIO.ElevatorIOInputs ElevatorIOInputs = new ElevatorIOInputs(); ;
+
     @AutoLog
     class ElevatorIOInputs{
         public double loadHeight = 0.0;
@@ -25,5 +27,7 @@ class ElevatorIOInputs{
     public default void updateInputs(ElevatorIO.ElevatorIOInputs inputs) {}
 
     public default void setBothElevatorMotorVoltages(double volts) {
+
+
     }
 }

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

@@ -5,14 +5,17 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.commands.ElevatorSetPointCommand;
 import frc.robot.constants.ElevatorConstants;
 import org.littletonrobotics.junction.Logger;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
 
 import static frc.robot.constants.ElevatorConstants.pulleyRadius;
 
 
 public class ElevatorIOSim implements ElevatorIO {
 
+
     private static final double LOOP_PERIOD_SECS = 0.02;
 
     private final DCMotorSim elevatorMotor1Sim = new DCMotorSim(
@@ -21,14 +24,16 @@ public class ElevatorIOSim implements ElevatorIO {
     private final DCMotorSim elevatorMotor2Sim = new DCMotorSim(
             LinearSystemId.createDCMotorSystem(ElevatorConstants.elevator2Gearbox, 0.025, ElevatorConstants.elevator2MotorReduction),
             ElevatorConstants.elevator2Gearbox);
-    private double elevatorAppliedVolts = 0.0;
+
+    private double elevatorAppliedVolts;
+
 
     @Override
     public void updateInputs(ElevatorIO.ElevatorIOInputs inputs) {
         elevatorMotor1Sim.update(LOOP_PERIOD_SECS);
         elevatorMotor2Sim.update(LOOP_PERIOD_SECS);
         Logger.recordOutput("ElevatorSubsystem/position", inputs.loadHeight);
-        inputs.loadHeight = (2 * Math.PI * pulleyRadius) / ((elevatorMotor1Sim.getAngularPositionRad() - inputs.lastEncoderPosition) * ElevatorConstants.gearingRatio);
+        inputs.loadHeight = (2 * Math.PI * pulleyRadius) * ((elevatorMotor1Sim.getAngularPositionRad() - inputs.lastEncoderPosition) * ElevatorConstants.gearingRatio);
         inputs.lastEncoderPosition = elevatorMotor1Sim.getAngularPositionRad();
 
         inputs.elevatorAppliedVolts = elevatorAppliedVolts;

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

@@ -1,20 +1,10 @@
 package frc.robot.subsystems.ElevatorSubsystem;
 
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.revrobotics.spark.SparkLowLevel;
-import com.revrobotics.spark.SparkMax;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj.Encoder;
 import frc.robot.constants.ElevatorConstants;
-import org.littletonrobotics.junction.Logger;
-
-import static frc.robot.constants.ElevatorConstants.L1;
 
 
 public class ElevatorSubsystem extends SubsystemBase {
@@ -23,7 +13,7 @@ public class ElevatorSubsystem extends SubsystemBase {
     public final PIDController elevatorFeedback = new PIDController(ElevatorConstants.kP, ElevatorConstants.kI, ElevatorConstants.kD);
     double maxVoltage = 12.0;
     public final TrapezoidProfile elevatorProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(5,5));
-    public TrapezoidProfile.State profileGoal = new TrapezoidProfile.State();// goal is the place where we want to go after already moving to another level. For motion profiling, we need to convert this to the setpoint or something idk I dont get paid enough for this crap I mean technically it is a -$350 profit//
+    public TrapezoidProfile.State profileGoal = new TrapezoidProfile.State(0,0);// goal is the place where we want to go after already moving to another level. For motion profiling, we need to convert this to the setpoint or something idk I dont get paid enough for this crap I mean technically it is a -$350 profit//
     public TrapezoidProfile.State profileSetPoint = new TrapezoidProfile.State();
     private final ElevatorEncoderIO elevatorEncoderIO;
     private final ElevatorIOInputsAutoLogged elevatorIOInputs;
@@ -51,6 +41,12 @@ public void moveElevatorToVelocity(double velocitySetpoint, double positionSetpo
         double feedforwardOutput = elevatorFeedforward.calculate(velocitySetpoint);
         double pidOutput = elevatorFeedback.calculate(encoderIOInputs.elevatorPosition, positionSetpoint);
         double totalOutput = feedforwardOutput + pidOutput;
-        elevatorIO.setBothElevatorMotorVoltages(totalOutput);
+        ElevatorIOSim.setBothElevatorMotorVoltages(totalOutput);
+    }
+
+    public double getTotalOutput(double velocitySetpoint, double positionSetpoint) {
+        double feedforwardOutput = elevatorFeedforward.calculate(velocitySetpoint);
+        double pidOutput = elevatorFeedback.calculate(encoderIOInputs.elevatorPosition, positionSetpoint);
+        return feedforwardOutput + pidOutput;
     }
 }