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

@@ -89,28 +89,24 @@ public class RobotContainer {
 			)
 		);
 
-		elevator.setDefaultCommand(
-			elevator.runManualElevator(
-				() -> -operator.getLeftY()
-			)
-		);
+		//elevator.setDefaultCommand(
+			//elevator.runAssistedElevator(operator::getLeftY)
+	//	);
 
-		
-		manipulatorPivot.setDefaultCommand(
-			manipulatorPivot.setSpeed(
-				() -> 0
-			)
-		);
-		
+		//elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+		elevator.setDefaultCommand(elevator.manualControl(() -> 0.0));
+
+		manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
 
 		manipulator.setDefaultCommand(
-			manipulator.runUntilCollected(
-				() -> 0
-			)
+			manipulator.defaultCommand()
 		);
 
+		//manipulatorPivot.setDefaultCommand(
+		//	manipulatorPivot.runAssistedPivot(operator::getRightY)
+		//);
+
 		//Driver inputs
-		/*
 		driver.start().whileTrue(
 			drivetrain.setXCommand()
 		);
@@ -132,18 +128,16 @@ public class RobotContainer {
 
 		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
-		*/
 
 		operator.povUp().whileTrue(
 			elevator.goToSetpoint(20)
 		);
 
-		/*
-		operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
+		//operator.a().whileTrue(elevator.manualControl(() -> 0.5));
 
-		operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
+		//operator.b().whileTrue(elevator.manualControl(() ->-0.1));
 
- 
+		/* 
 		//Operator inputs
 		operator.povUp().onTrue(
 			moveManipulator(
@@ -206,33 +200,36 @@ public class RobotContainer {
 
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(0, 0)
+			.withPosition(0, 1)
 			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(2, 0)
+			.withPosition(2, 1)
 			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Climber Pivot Position", climberPivot::getEncoderPosition)
 			.withSize(2, 1)
-			.withPosition(2, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-			
-		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue)
-			.withSize(2, 1)
-			.withPosition(0, 1)
+			.withPosition(2, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addBoolean("Coral Sensor", manipulator::getCoralBeamBreak)
-			.withSize(1, 1)
-			.withPosition(4, 0)
-			.withWidget(BuiltInWidgets.kBooleanBox);
-
-		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch)
 			.withSize(1, 1)
 			.withPosition(4, 1)
 			.withWidget(BuiltInWidgets.kBooleanBox);
+
+		sensorTab.addDouble("gyro angle", drivetrain::getGyroValue);
+
+		sensorTab.addBoolean("bottom limit switch", elevator::getBottomLimitSwitch);
+
+		sensorTab.addDouble("elevator position", elevator::getEncoderPosition);
+
+		/* 
+		sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
+			.withSize(1, 1)
+			.withPosition(4, 0)
+			.withWidget(BuiltInWidgets.kBooleanBox);
+			*/
   }
 
 	public Command getAutonomousCommand() {
@@ -243,13 +240,12 @@ public class RobotContainer {
 	 * Moves the elevator and arm to the coral intake position, then runs the manipulator until collected
 	 * @return Moves the elevator and arm, then intakes coral
 	 */
-	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
 			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(() -> .5));
+			.andThen(manipulator.runUntilCollected(() -> 1));
 		}
 
 	/**
@@ -258,7 +254,6 @@ public class RobotContainer {
 	 * @param l2 Is the algae on L2? (True = L2, False = L3)
 	 * @return Moves the elevator and arm then intakes algae
 	 */
-	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
 			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
@@ -330,7 +325,6 @@ public class RobotContainer {
 		);
 	}
 
-	@SuppressWarnings("unused")
 	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
@@ -360,7 +354,6 @@ public class RobotContainer {
 			.andThen(manipulatorPivot.goToSetpoint(armPosition));
 	}
 
-	@SuppressWarnings("unused")
 	private Command startingConfig() {
 		return moveManipulatorUtil(0, 0, false, true)
 			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));

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

@@ -21,7 +21,7 @@ public class ElevatorConstants {
 
     public static final int kCurrentLimit = 40;
 
-    public static final double kPositionControllerP = 0.1;
+    public static final double kPositionControllerP = 0.3;
     public static final double kPositionControllerI = 0;
     public static final double kPositionControllerD = 0;
 
@@ -74,11 +74,9 @@ public class ElevatorConstants {
         motorConfig
             .smartCurrentLimit(kCurrentLimit)
             .idleMode(kIdleMode)
-            .inverted(true);
+            .inverted(false);
         motorConfig.encoder
-            //.inverted(true)
-            .positionConversionFactor(kEncoderConversionFactor);  
-        /*        
+            .positionConversionFactor(kEncoderConversionFactor);   
         motorConfig.closedLoop
             .p(kPositionControllerP)
             .i(kPositionControllerI)
@@ -88,7 +86,6 @@ public class ElevatorConstants {
             .maxAcceleration(kMaxAcceleration)
             .maxVelocity(kMaxVelocity)
             .allowedClosedLoopError(kAllowedError);
-        */
     }
 
     public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig();

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

@@ -15,6 +15,8 @@ public class ClimberPivot extends SubsystemBase {
 
     private RelativeEncoder neoEncoder;
 
+    //private DigitalInput cageLimitSwitch;
+
     public ClimberPivot() {
         pivotMotor = new SparkMax(
             ClimberPivotConstants.kPivotMotorID, 
@@ -28,6 +30,8 @@ public class ClimberPivot extends SubsystemBase {
         );
 
         neoEncoder = pivotMotor.getEncoder();
+
+        //cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID);
     }
 
     public Command runPivot(double speed) {
@@ -49,6 +53,15 @@ public class ClimberPivot extends SubsystemBase {
         }).until(() -> neoEncoder.getPosition() >= setpoint);
     }
 
+    /**
+     * Returns the limit switch attached to the climber. Detects if the cage is present
+     * 
+     * @return Is the cage in the climber
+     */
+   // public boolean getCageLimitSwitch() {
+    //    return cageLimitSwitch.get();
+    //}
+
     public double getEncoderPosition() {
         return neoEncoder.getPosition();
     }

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

@@ -14,7 +14,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 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;
@@ -24,14 +23,12 @@ public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
     protected SparkMax elevatorMotor2;
 
-    //private SparkClosedLoopController elevatorClosedLoop;
+    private SparkClosedLoopController elevatorClosedLoop;
 
     protected RelativeEncoder encoder;
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidController;
-
     private ElevatorFeedforward feedForward;
     
     public Elevator() {
@@ -51,26 +48,27 @@ public class Elevator extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
+        elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
+
         elevatorMotor2.configure(
             ElevatorConstants.motorConfig2.follow(ElevatorConstants.kElevatorMotor1ID), 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        //elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
-
         encoder = elevatorMotor1.getEncoder();
 
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidController = new PIDController(
-            ElevatorConstants.kPositionControllerP, 
-            ElevatorConstants.kPositionControllerI,
-            ElevatorConstants.kPositionControllerD
+        /* 
+        velocityController = new PIDController(
+            ElevatorConstants.kVelocityControllerP, 
+            ElevatorConstants.kVelocityControllerI, 
+            ElevatorConstants.kVelocityControllerD
         );
-        pidController.setTolerance(ElevatorConstants.kAllowedError);
+        */
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -113,15 +111,43 @@ public class Elevator extends SubsystemBase {
      * @param speed The speed at which the elevator translates
      * @return Sets motor voltage to translate the elevator and maintain position
      */
-    public Command runManualElevator(DoubleSupplier speed) {
+    public Command runManualElevator(double speed) {
         return run(() -> {
-            //elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
-            elevatorMotor1.set(speed.getAsDouble());
+            elevatorMotor1.set(speed);
+        });
+    }
+
+    /**
+     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed How fast the elevator moves
+     * @return Sets motor voltage to move the elevator relative to the speed parameter
+     *
+    public Command runAssistedElevator(DoubleSupplier speed) {
+        return run(() -> {
+            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
+
+            double voltsOut = velocityController.calculate(
+                encoder.getVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(realSpeedTarget);
+
+            elevatorMotor1.setVoltage(voltsOut);
+        }).until(
+            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
+    }
+            */
+    
+    public Command manualControl(DoubleSupplier speed){
+        return run(() -> {
+            elevatorClosedLoop.setReference(speed.getAsDouble(), ControlType.kDutyCycle);
+
         });
     }
 
     /**
      * Moves the elevator to a target destination (setpoint). 
+     * If the setpoint is 0, the elevator will creep down to hit the limit switch
      * 
      * @param setpoint Target destination of the subsystem
      * @param timeout Time to achieve the setpoint before quitting
@@ -135,20 +161,44 @@ public class Elevator extends SubsystemBase {
         );
 
         return run(() -> {
-            elevatorMotor1.setVoltage(
-                pidController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0)
-            );
-            /*
+
             elevatorClosedLoop.setReference(clampedSetpoint,
                 SparkBase.ControlType.kMAXMotionPositionControl,
                 ClosedLoopSlot.kSlot0,
                 feedForward.calculate(0)
             );
-            */
+
         });
+/* 
+        if (clampedSetpoint == 0) {
+            return run(() -> {
+                double voltsOut = positionController.calculate(
+                    encoder.getPosition(),
+                    clampedSetpoint
+                ) + feedForward.calculate(0);
+    
+                elevatorMotor1.setVoltage(voltsOut);
+            }).until(
+                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
+            ).withTimeout(timeout)
+                .andThen(Commands.either(
+                    runAssistedElevator(() -> 0),
+                    runAssistedElevator(() -> -.2), 
+                    bottomLimitSwitch::get
+                )).withTimeout(timeout);
+        } else {
+            return run(() -> {
+                double voltsOut = positionController.calculate(
+                    encoder.getPosition(),
+                    clampedSetpoint
+                ) + feedForward.calculate(0);
+    
+                elevatorMotor1.setVoltage(voltsOut);
+            }).until(
+                () -> positionController.atSetpoint() || bottomLimitSwitch.get()
+            ).withTimeout(timeout);
+        }
+            */
 
     }
 

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

@@ -16,6 +16,7 @@ public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
     private DigitalInput coralBeamBreak;
+  //  private DigitalInput algaeBeamBreak; 
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -30,6 +31,7 @@ public class Manipulator extends SubsystemBase {
         );
 
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
+      //  algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
     /**
@@ -40,7 +42,8 @@ public class Manipulator extends SubsystemBase {
      */
     public Command defaultCommand() {
         return run(() -> {
-            runUntilCollected(() -> 0.1);
+            //runUntilCollected(0.1);
+            manipulatorMotor.set(0);
             });
     }
 
@@ -74,19 +77,6 @@ public class Manipulator extends SubsystemBase {
         }).until(() -> !coralBeamBreak.get());
     }
 
-    /**
-     * Runs the manipulator in a way that will bring the coral to a reliable holding position
-     * 
-     * @return Returns a command that will position the coral to a known location
-     */
-    public Command indexCoral() {
-        return run(() -> {
-            runUntilCollected(() -> 0.5)
-                .andThen(runManipulator(() -> .1, false))
-                .until(() -> getCoralBeamBreak());
-        });
-    }
-
     public boolean getCoralBeamBreak() {
         return coralBeamBreak.get();
     }

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

@@ -1,9 +1,6 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkMax;
-
-import java.util.function.DoubleSupplier;
-
 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkAbsoluteEncoder;
 import com.revrobotics.spark.SparkBase;
@@ -69,12 +66,35 @@ public class ManipulatorPivot extends SubsystemBase {
         return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
     }
 
-    public Command setSpeed(DoubleSupplier speed) {
+    /**
+     * A manual rotation command that will move the elevator using a consistent velocity disregarding direction
+     * 
+     * @param speed The velocity at which the arm rotates
+     * @return Sets motor voltage to achieve the target velocity
+     */
+    /* 
+    public Command runAssistedPivot(DoubleSupplier speed) {
+        double clampedSpeed = MathUtil.clamp(
+            speed.getAsDouble(), 
+            -1, 
+            1
+        );
+
         return run(() -> {
-            armMotor.set(speed.getAsDouble());
+            double realSpeedTarget = clampedSpeed * ManipulatorPivotConstants.kPivotMaxVelocity;
+
+            double voltsOut = velocityController.calculate(
+                getEncoderVelocity(), 
+                realSpeedTarget
+            ) + feedForward.calculate(
+                getEncoderPosition(), 
+                getEncoderVelocity()
+            );
+
+            armMotor.setVoltage(voltsOut);
         });
     }
-    
+*/
     /**
      * Moves the arm to a target destination (setpoint)
      *