added a few manual controls for arm and elevator

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

@@ -78,7 +78,7 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.goToSetpoint(0, 1)
+			elevator.runElevator(operator::getLeftY)
 		);
 
 		indexer.setDefaultCommand(

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

@@ -5,4 +5,6 @@ public class ArmConstants {
     public static final int kCANcoderID = 0;
 
     public static final double kEncoderConversionFactor = 0;
+
+    public static final double kArmMaxVelocity = 0;
 }

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

@@ -11,20 +11,29 @@ 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 int kElevatorMotor1ID = 0;
+    public static final int kElevatorMotor2ID = 0;
+
+    public static final int kTopLimitSwitchID = 0;
+    public static final int kBottomLimitSwitchID = 0;
 
     public static final double kEncoderConversionFactor = 0;
 
     public static final int kMotorAmpsMax = 0;
 
-    public static final double kPIDControllerP = 0;
+    public static final double kPositionControllerP = 0;
-    public static final double kPIDControllerI = 0;
+    public static final double kPositionControllerI = 0;
-    public static final double kPIDControllerD = 0;
+    public static final double kPositionControllerD = 0;
+    
+    public static final double kVelocityControllerP = 0;
+    public static final double kVelocityControllerI = 0;
+    public static final double kVelocityControllerD = 0;
 
     public static final double kFeedForwardS = 0;
     public static final double kFeedForwardG = 0;
     public static final double kFeedForwardV = 0;
 
+    public static final double kElevatorMaxVelocity = 0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -15,7 +17,9 @@ public class Arm extends SubsystemBase {
 
     private CANcoder canCoder;
 
-    private PIDController pidController;
+    private PIDController positionController;
+    private PIDController velocityController;
+
     private ArmFeedforward feedForward;
 
     public Arm() {
@@ -27,9 +31,26 @@ public class Arm extends SubsystemBase {
         canCoder = new CANcoder(ArmConstants.kCANcoderID);
     }
 
+    //manual command that keeps ouput speed consistent no matter the direction
+    public Command runArm(DoubleSupplier speed) {
+        return run(() -> {
+            double realSpeedTarget = speed.getAsDouble() * ArmConstants.kArmMaxVelocity;
+
+            double voltsOut = velocityController.calculate(
+                rotationsToRadians(canCoder.getVelocity().getValueAsDouble()), 
+                realSpeedTarget
+            ) + feedForward.calculate(
+                rotationsToRadians(canCoder.getPosition().getValueAsDouble()), 
+                canCoder.getVelocity().getValueAsDouble()
+            );
+
+            armMotor.setVoltage(voltsOut);
+        });
+    }
+
     public Command goToSetpoint(double setpoint, double timeout) {
         return run(() -> {
-            double voltsOut = pidController.calculate(
+            double voltsOut = positionController.calculate(
                 canCoder.getPosition().getValueAsDouble(), 
                 setpoint
             ) + feedForward.calculate(
@@ -38,6 +59,10 @@ public class Arm extends SubsystemBase {
             );
 
             armMotor.setVoltage(voltsOut);
-        }).until(pidController::atSetpoint).withTimeout(timeout);
+        }).until(positionController::atSetpoint).withTimeout(timeout);
+    }
+
+    protected double rotationsToRadians(double rotations) {
+        return rotations * 2 * Math.PI;
     }
 }

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -8,22 +10,33 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
 
 public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
+    protected SparkMax elevatorMotor2;
 
     protected RelativeEncoder encoder;
 
-    private PIDController pidController;
+    private DigitalInput topLimitSwitch;
+    private DigitalInput bottomLimitSwitch;
+
+    private PIDController positionController;
+    private PIDController velocityController;
 
     private ElevatorFeedforward feedForward;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
-            ElevatorConstants.kElevatorMotorID, 
+            ElevatorConstants.kElevatorMotor1ID, 
+            MotorType.kBrushless
+        );
+        
+        elevatorMotor2 = new SparkMax(
+            ElevatorConstants.kElevatorMotor2ID,
             MotorType.kBrushless
         );
 
@@ -33,13 +46,32 @@ public class Elevator extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
+        elevatorMotor2.configure(
+            ElevatorConstants.motorConfig.follow(ElevatorConstants.kElevatorMotor1ID), 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
 
         encoder = elevatorMotor1.getEncoder();
 
-        pidController = new PIDController(
+        topLimitSwitch = new DigitalInput(
-            ElevatorConstants.kPIDControllerP, 
+            ElevatorConstants.kTopLimitSwitchID
-            ElevatorConstants.kPIDControllerI, 
+        );
-            ElevatorConstants.kPIDControllerD
+
+        bottomLimitSwitch = new DigitalInput(
+            ElevatorConstants.kBottomLimitSwitchID
+        );
+
+        positionController = new PIDController(
+            ElevatorConstants.kPositionControllerP, 
+            ElevatorConstants.kPositionControllerI, 
+            ElevatorConstants.kPositionControllerD
+        );
+
+        velocityController = new PIDController(
+            ElevatorConstants.kVelocityControllerP, 
+            ElevatorConstants.kVelocityControllerI, 
+            ElevatorConstants.kVelocityControllerD
         );
 
         feedForward = new ElevatorFeedforward(
@@ -49,20 +81,32 @@ public class Elevator extends SubsystemBase {
         );
     }
 
-    public Command runElevator(double speed) {
+    //manual command that keeps ouput speed consistent no matter the direction
+    public Command runElevator(DoubleSupplier speed) {
         return run(() -> {
-            elevatorMotor1.set(speed);
+            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
-        });
+
+            double voltsOut = velocityController.calculate(
+                encoder.getVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(realSpeedTarget);
+
+            elevatorMotor1.setVoltage(voltsOut);
+        }).until(() -> topLimitSwitch.get() || bottomLimitSwitch.get());
     }
+
     
+    //go to setpoint command
     public Command goToSetpoint(double setpoint, double timeout) {
         return run(() -> {
-            double voltsOut = pidController.calculate(
+            double voltsOut = positionController.calculate(
                 encoder.getPosition(),
                 setpoint
             ) + feedForward.calculate(0);
 
             elevatorMotor1.setVoltage(voltsOut);
-        }).until(pidController::atSetpoint).withTimeout(timeout);
+        }).until(
+            () -> positionController.atSetpoint() || topLimitSwitch.get() || bottomLimitSwitch.get()
+        ).withTimeout(timeout);
     }
 }