Added loggers and var offset

build.gradle

@@ -115,7 +115,11 @@ wpi.sim.addDriverstation()
 // in order to make them all available at runtime. Also adding the manifest so WPILib
 // knows where to look for our Robot Class.
 jar {
-    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    from {
+        configurations.runtimeClasspath.collect {
+            it.isDirectory() ? it : zipTree(it)
+        }
+    }
     from sourceSets.main.allSource
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
     duplicatesStrategy = DuplicatesStrategy.INCLUDE

src/main/java/frc/robot/Constants.java

@@ -34,6 +34,7 @@ public static enum Mode {
     /** Replaying from a log file. */
     REPLAY
   }
+
   public static enum CAN {
     ElevatorCan
   }

src/main/java/frc/robot/Main.java

@@ -14,7 +14,6 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.RobotBase;
-import frc.robot.subsystems.drive.ElevatorSubsystem;
 
 /**
  * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
@@ -23,6 +22,7 @@
  */
 public final class Main {
   private Main() {}
+
   /**
    * Main initialization function. Do not perform any initialization here.
    *

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

@@ -116,7 +116,7 @@ private void configureButtonBindings() {
             drive,
             () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
-            () ->  controller.getRightX()));
+            () -> controller.getRightX()));
 
     // Lock to 0° when A button is held
     controller

src/main/java/frc/robot/commands/MoveElevatorCommand.kt

@@ -16,6 +16,7 @@ class MoveElevatorCommand : Command() {
     override fun initialize() {
         ElevatorSubsystem
     }
+
     override fun execute() {
         ElevatorSubsystem.moveToNextPosition()
     }
@@ -25,5 +26,12 @@ class MoveElevatorCommand : Command() {
         return false
     }
 
-    override fun end(interrupted: Boolean) {}
+override fun end(interrupted: Boolean) {
+    if (isFinished){
+
+        return println("end")
+
+    }
 }
+
+}

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

@@ -10,8 +10,10 @@ import org.littletonrobotics.junction.Logger
 
 
 object ElevatorSubsystem : SubsystemBase() {
-    // With eager singleton initialization, any static variables/fields used in the 
-    // constructor must appear before the "INSTANCE" variable so that they are initialized 
+    var offset = 0.0
+
+    // With eager singleton initialization, any static variables/fields used in the
+    // constructor must appear before the "INSTANCE" variable so that they are initialized
     // before the constructor is called when the "INSTANCE" variable initializes.
     /**
      * Returns the Singleton instance of this ElevatorSubsystem. This static method
@@ -26,28 +28,37 @@ object ElevatorSubsystem : SubsystemBase() {
     val instance: ElevatorSubsystem = ElevatorSubsystem
     val leader = TalonFX(1, CANBus("1"))
     var digit: Int = 0
+
     val Positions = arrayOf(
         ElevatorConstants.Position.L1,
         ElevatorConstants.Position.L2,
         ElevatorConstants.Position.L3,
         ElevatorConstants.Position.L4
     )
+
     private var currentPosition:Double? = null
     private var desiredPosition:ElevatorConstants.Position = Positions[digit]
+
     fun getDesiredPosition():ElevatorConstants.Position {
         return desiredPosition
     }
+
     fun moveToNextPosition() {
         leader.setPosition(Positions[digit % 4].Height)
         println(Positions[digit])
         digit++
-
     }
+
     override fun periodic(){
         if(currentPosition == desiredPosition.Height){
             println("At desired position")
-            Logger.recordOutput("e",leader.position)
         }
+
+        Logger.recordOutput("Elevator/DesiredPosition", desiredPosition)
+        Logger.recordOutput("Elevator/Offset", offset)
+
+        Logger.recordOutput("Elevator/Leader/Position", leader.position.valueAsDouble)
+        Logger.recordOutput("Elevator/Leader/Velocity", leader.velocity.valueAsDouble)
     }
 
 }

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -259,8 +259,8 @@ public void setTurnPosition(Rotation2d rotation) {
     turnTalon.setControl(
         switch (constants.SteerMotorClosedLoopOutput) {
           case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations());
-          case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition(
-              rotation.getRotations());
+          case TorqueCurrentFOC ->
+              positionTorqueCurrentRequest.withPosition(rotation.getRotations());
         });
   }
 }