minor subsystem changes

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

@@ -61,7 +61,7 @@ public class RobotContainer {
 		);
 
 		climberPivot.setDefaultCommand(
-			climberPivot.goToAngle(0)
+			climberPivot.goToAngle(0, 1)
 		);
 
 		climberRollers.setDefaultCommand(

src/main/java/frc/robot/constants/ArmConstants.java

@@ -3,4 +3,6 @@ package frc.robot.constants;
 public class ArmConstants {
     public static final int kArmMotorID = 0;
     public static final int kCANcoderID = 0;
+
+    public static final double kEncoderConversionFactor = 0;
 }

src/main/java/frc/robot/constants/ClimberPivotConstants.java

@@ -5,6 +5,8 @@ public class ClimberPivotConstants {
 
     public static final int kClimberLimitSwitchID = 0;
 
+    public static final double kEncoderConversionFactor = 0;
+
     public static final double kPIDControllerP = 0;
     public static final double kPIDControllerI = 0;
     public static final double kPIDControllerD = 0;

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -1,8 +1,22 @@
 package frc.robot.constants;
 
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+
 public class ElevatorConstants {
     public static final int kElevatorMotorID = 0;
 
+    public static final double kEncoderConversionFactor = 0;
+
+    public static final int kMotorAmpsMax = 0;
+
     public static final double kPIDControllerP = 0;
     public static final double kPIDControllerI = 0;
     public static final double kPIDControllerD = 0;
@@ -10,4 +24,32 @@ public class ElevatorConstants {
     public static final double kFeedForwardS = 0;
     public static final double kFeedForwardG = 0;
     public static final double kFeedForwardV = 0;
+
+
+    // 1, 7, 10 are the defaults for these, change as necessary
+    public static final double kSysIDRampRate = 1;
+    public static final double kSysIDStepVolts = 7;
+    public static final double kSysIDTimeout = 10;
+
+    // This should be kCoast for SysID, any other time it can be kBrake if you want
+    public static final IdleMode kIdleMode = IdleMode.kCoast;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
+    
+    public static final SysIdRoutine.Config kSysIDConfig = new Config(
+        Volts.of(kSysIDRampRate).per(Second), 
+        Volts.of(kSysIDStepVolts), 
+        Seconds.of(kSysIDTimeout)
+    );
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static {
+        motorConfig
+            .smartCurrentLimit(kMotorAmpsMax)
+            .idleMode(kIdleMode);
+        motorConfig.encoder
+            .positionConversionFactor(kEncoderConversionFactor)
+            .velocityConversionFactor(kEncoderConversionFactor / 60.0);
+    }
 }

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

@@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ArmConstants;
 
 public class Arm extends SubsystemBase {
-    private SparkMax ArmMotor;
+    private SparkMax armMotor;
 
     private CANcoder canCoder;
 
@@ -19,7 +19,7 @@ public class Arm extends SubsystemBase {
     private ArmFeedforward feedForward;
 
     public Arm() {
-        ArmMotor = new SparkMax(
+        armMotor = new SparkMax(
             ArmConstants.kArmMotorID, 
             MotorType.kBrushless
         );
@@ -37,7 +37,7 @@ public class Arm extends SubsystemBase {
                 canCoder.getVelocity().getValueAsDouble()
             );
 
-            ArmMotor.setVoltage(voltsOut);
+            armMotor.setVoltage(voltsOut);
         }).until(pidController::atSetpoint).withTimeout(timeout);
     }
 }

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

@@ -1,6 +1,6 @@
 package frc.robot.subsystems;
 
-import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
@@ -13,7 +13,7 @@ import frc.robot.constants.ClimberPivotConstants;
 public class ClimberPivot extends SubsystemBase {
     private SparkMax pivotMotor;
 
-    private AbsoluteEncoder neoEncoder;
+    private RelativeEncoder neoEncoder;
 
     private DigitalInput cageLimitSwitch;
 
@@ -25,9 +25,9 @@ public class ClimberPivot extends SubsystemBase {
             MotorType.kBrushless
         );
 
-        neoEncoder = pivotMotor.getAbsoluteEncoder();
+        neoEncoder = pivotMotor.getEncoder();
 
-        cageLimitSwitch = new DigitalInput(0);
+        cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
 
         pidController = new PIDController(
             ClimberPivotConstants.kPIDControllerP,
@@ -42,7 +42,7 @@ public class ClimberPivot extends SubsystemBase {
         });
     }
 
-    public Command goToAngle(double setpoint) {
+    public Command goToAngle(double setpoint, double timeout) {
         return run(() -> {
             pivotMotor.set(
                 pidController.calculate(
@@ -50,7 +50,7 @@ public class ClimberPivot extends SubsystemBase {
                     setpoint
                 )
             );
-        });
+        }).withTimeout(timeout);
     }
 
     public boolean getCageLimitSwitch() {

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

@@ -1,7 +1,9 @@
 package frc.robot.subsystems;
 
-import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
@@ -11,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
 
 public class Elevator extends SubsystemBase {
-    private SparkMax elevatorMotor1;
+    protected SparkMax elevatorMotor1;
 
-    private AbsoluteEncoder encoder;
+    protected RelativeEncoder encoder;
 
     private PIDController pidController;
 
@@ -25,7 +27,14 @@ public class Elevator extends SubsystemBase {
             MotorType.kBrushless
         );
 
-        encoder = elevatorMotor1.getAbsoluteEncoder();
+        elevatorMotor1.configure(
+            ElevatorConstants.motorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+
+        encoder = elevatorMotor1.getEncoder();
 
         pidController = new PIDController(
             ElevatorConstants.kPIDControllerP, 

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

@@ -31,7 +31,6 @@ public class Indexer extends SubsystemBase {
     public Command indexCoral(double speed) {
         return run(() -> {
             indexerMotor.set(speed);
-        })
+        }).until(indexerBeamBreak::get);
-        .until(indexerBeamBreak::get);
     }
 }

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

@@ -33,14 +33,12 @@ public class Manipulator extends SubsystemBase {
     public Command runUntilCollected(double speed, boolean coral) {
         return run(() -> {
             manipulatorMotor.set(coral ? speed : speed * -1);
-        })
+        }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
-        .until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
     }
 
     public Command indexCoral(double speed) {
         return run(() -> {
             manipulatorMotor.set(speed);
-        })
+        }).until(coralBeamBreak::get);
-        .until(coralBeamBreak::get);
     }
 }

src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java

@@ -0,0 +1,62 @@
+package frc.robot.subsystems.sysid;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
+import edu.wpi.first.units.measure.MutDistance;
+import edu.wpi.first.units.measure.MutLinearVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.subsystems.Elevator;
+
+public class ElevatorSysID extends Elevator {
+    private MutVoltage appliedVoltage;
+
+    private MutDistance elevatorPosition;
+
+    private MutLinearVelocity elevatorVelocity;
+
+    private SysIdRoutine routine;
+
+    public ElevatorSysID() {
+        super();
+
+        appliedVoltage = Volts.mutable(0);
+
+        elevatorPosition = Meters.mutable(0);
+
+        elevatorVelocity = MetersPerSecond.mutable(0);
+
+        routine = new SysIdRoutine(
+            ElevatorConstants.kSysIDConfig,
+            new SysIdRoutine.Mechanism(
+                elevatorMotor1::setVoltage,
+                (log) -> {
+                    log.motor("elevatorMotor1")
+                        .voltage(appliedVoltage.mut_replace(
+                            elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
+                        ))
+                        .linearPosition(elevatorPosition.mut_replace(
+                            encoder.getPosition(), Meters
+                        ))
+                        .linearVelocity(elevatorVelocity.mut_replace(
+                            encoder.getVelocity(), MetersPerSecond
+                        ));
+                }, 
+                this
+            )
+        );
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+}