Still resolving merge conflict

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(
@@ -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,15 +11,23 @@ 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;

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;
@@ -11,15 +13,17 @@ 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;
 
-    private PIDController pidController;
+    private PIDController positionController;
+    private PIDController velocityController;
+
     private ArmFeedforward feedForward;
 
     public Arm() {
-        ArmMotor = new SparkMax(
+        armMotor = new SparkMax(
             ArmConstants.kArmMotorID, 
             MotorType.kBrushless
         );
@@ -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(
@@ -37,7 +58,11 @@ public class Arm extends SubsystemBase {
                 canCoder.getVelocity().getValueAsDouble()
             );
 
-            ArmMotor.setVoltage(voltsOut);
+            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/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,29 +1,44 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 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.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 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,12 +48,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(
@@ -48,20 +83,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);
     }
 }

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);
     }
 }