Rework done, only a proof of concept, shouldn't be used without a lot of free time

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

@@ -8,6 +8,7 @@ import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.subsystems.ManipulatorPivot;
 import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.ClimberPivot;
@@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
@@ -53,14 +55,14 @@ public class RobotContainer {
 
 		climberRollers = new ClimberRollers();
 
+		manipulator = new Manipulator();
+
 		//vision = new Vision(drivetrain::getGyroValue);
 		drivetrain = new Drivetrain();
 
-		elevator = new Elevator();
+		elevator = new Elevator(manipulator::getAlgaePhotoswitch);
 		//elevator = new ElevatorSysID();
 
-		manipulator = new Manipulator();
-
 		manipulatorPivot = new ManipulatorPivot();
 
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
@@ -104,11 +106,6 @@ public class RobotContainer {
 			)
 		);
 		
-		elevator.setDefaultCommand(
-			elevator.maintainPosition()
-		);
-		
-		
 		manipulatorPivot.setDefaultCommand(
 			manipulatorPivot.maintainPosition()
 		);
@@ -147,28 +144,28 @@ public class RobotContainer {
 		//Operator inputs
 		operator.povUp().onTrue(
 			safeMoveManipulator(
-				ElevatorConstants.kL4Position, 
+				ElevatorPositions.kL4, 
 				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
 			safeMoveManipulator(
-				ElevatorConstants.kL3Position, 
+				ElevatorPositions.kL3, 
 				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
 			safeMoveManipulator(
-				ElevatorConstants.kL2Position, 
+				ElevatorPositions.kL2, 
 				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
 			safeMoveManipulator(
-				ElevatorConstants.kL1Position, 
+				ElevatorPositions.kL1, 
 				ManipulatorPivotConstants.kL1Position
 			)
 		);
@@ -185,8 +182,8 @@ public class RobotContainer {
 			algaeIntakeRoutine(false)
 		);
 
-		operator.y().whileTrue(
+		operator.y().onTrue(
-			elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
+			elevator.setTargetPosition(ElevatorPositions.kL2)
 		);
 	
 
@@ -272,7 +269,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
-			ElevatorConstants.kCoralIntakePosition, 
+			ElevatorPositions.kCoralIntake, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
 			.andThen(manipulator.runUntilCollected(() -> .5));
@@ -287,7 +284,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
-			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
+			l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae, 
 			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
 			.andThen(manipulator.runUntilCollected(() -> 1));
@@ -300,10 +297,10 @@ public class RobotContainer {
 	 * @param armPosition The target rotation of the arm
 	 * @return Moves the elevator and arm to the setpoints using the most efficient path
 	 */
-	private Command moveManipulator(double elevatorPosition, double armPosition) {
+	private Command moveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
 		// If the elevator current and target positions are above the brace, or the arm current and target position is in
 		// front of the brace, move together
-		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition.getPosition())) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
 			return moveManipulatorUtil(elevatorPosition, armPosition, false, false);
 		// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
 		// then the elevator, then the arm again
@@ -332,7 +329,7 @@ public class RobotContainer {
 	 * @param sequential Does the elevator and arm move separately? (True = .andThen, False = .alongWith)
 	 * @return Moves the elevator and arm to the setpoints
 	 */
-	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+	private Command moveManipulatorUtil(ElevatorPositions elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
 		
 		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
 			armPosition = MathUtil.clamp(
@@ -344,13 +341,16 @@ public class RobotContainer {
 		
 		return Commands.either(
 			Commands.either(
-				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
+				elevator.setTargetPosition(elevatorPosition).andThen(
-				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
+					new WaitUntilCommand(elevator::atSetpoint),
+					manipulatorPivot.goToSetpoint(() -> armPosition)
+				),
+				elevator.setTargetPosition(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
 				() -> sequential
 			), 
 			Commands.either(
-				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.setTargetPosition(elevatorPosition)), 
-				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
@@ -358,18 +358,24 @@ public class RobotContainer {
 	}
 
 	@SuppressWarnings("unused")
-	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
+	private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
-				elevator.goToSetpoint(() -> elevatorPosition),
+				elevator.setTargetPosition(elevatorPosition),
+				new WaitUntilCommand(elevator::atSetpoint),
 				manipulatorPivot.goToSetpoint(() -> armPosition));
 			
 		}else{
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
-				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				elevator.setTargetPosition(elevatorPosition),
-				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
+				new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()),
+				Commands.parallel(
+					manipulatorPivot.goToSetpoint(() -> armPosition), 
+					elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint))
+				)
+			);
 		}
 		
 	}
@@ -382,14 +388,14 @@ public class RobotContainer {
 	 * @return Moves the elevator and arm to the setpoints
 	 */
 	@SuppressWarnings("unused")
-	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
+	private Command safeMoveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
 		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
 			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 	}
 
 	@SuppressWarnings("unused")
 	private Command startingConfig() {
-		return moveManipulatorUtil(0, 0, false, true)
+		return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true)
 			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
 	}
 }

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

@@ -11,6 +11,34 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ElevatorConstants {
+    public enum ElevatorControlMode {
+        kPID,
+        kManualMaintain
+    }
+
+    public enum ElevatorPositions {
+        kStartingPosition(0),
+        kCoralIntake(0),
+        kL1(0),
+        kL2(14.5),
+        kL2Algae(22),
+        kL3(29),
+        kL3Algae(39),
+        kL4(53),
+        kL4Transition(40),
+        kProcessor(4);
+
+        private double position;
+
+        private ElevatorPositions(double position) {
+            this.position = position;
+        }
+
+        public double getPosition() {
+            return position;
+        }
+    }
+
     public static final int kElevatorMotor1ID = 8;
     public static final int kElevatorMotor2ID = 6;
 
@@ -36,18 +64,13 @@ public class ElevatorConstants {
     public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
     public static final double kFeedForwardV = 0.12;   // calculated value 0.12
 
+    public static final double kFeedForwardAlgaeHeldS = 0;
+    public static final double kFeedForwardAlgaeHeldG = 0;
+    public static final double kFeedForwardAlgaeHeldV = 0;
+
     public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
     public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
-    public static final double kCoralIntakePosition = 0;
-    public static final double kL1Position = 0;
-    public static final double kL2Position = 14.5;
-    public static final double kL3Position = 29.0;
-    public static final double kL4Position = 53.0;
-    public static final double kL4TransitionPosition = 40.0;
-    public static final double kL2AlgaePosition = 22.0;
-    public static final double kL3AlgaePosition = 39.0;
-    public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 0;
     public static final double kMaxHeight = 47.5; //actual is 53

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

@@ -4,7 +4,9 @@ import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ManipulatorConstants {
     public static final int kManipulatorMotorID = 12;
+
     public static final int kCoralBeamBreakID = 2;
+    public static final int kAlgaePhotoswitchID = 3;
     
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 }

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
@@ -15,6 +16,8 @@ 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;
+import frc.robot.constants.ElevatorConstants.ElevatorControlMode;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 
 public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
@@ -22,13 +25,20 @@ public class Elevator extends SubsystemBase {
 
     protected RelativeEncoder encoder;
 
+    private BooleanSupplier hasAlgae;
+
     private DigitalInput bottomLimitSwitch;
 
     private PIDController pidController;
 
     private ElevatorFeedforward feedForward;
+    private ElevatorFeedforward algaeHeldFeedForward;
 
-    public Elevator() {
+    private ElevatorControlMode mode;
+
+    private ElevatorPositions currentTargetPosition;
+    
+    public Elevator(BooleanSupplier hasAlgae) {
         elevatorMotor1 = new SparkMax(
             ElevatorConstants.kElevatorMotor1ID, 
             MotorType.kBrushless
@@ -53,6 +63,8 @@ public class Elevator extends SubsystemBase {
 
         encoder = elevatorMotor1.getEncoder();
 
+        this.hasAlgae = hasAlgae;
+
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
@@ -71,6 +83,16 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
+
+        algaeHeldFeedForward = new ElevatorFeedforward(
+            ElevatorConstants.kFeedForwardAlgaeHeldS, 
+            ElevatorConstants.kFeedForwardAlgaeHeldG, 
+            ElevatorConstants.kFeedForwardAlgaeHeldV
+        );
+
+        mode = ElevatorControlMode.kPID;
+
+        currentTargetPosition = ElevatorPositions.kCoralIntake;
     }
 
     @Override
@@ -78,6 +100,27 @@ public class Elevator extends SubsystemBase {
         if (!getBottomLimitSwitch()) {
             encoder.setPosition(0);
         }
+
+        if(mode == ElevatorControlMode.kPID) {
+            if (!pidController.atSetpoint()) {
+                elevatorMotor1.setVoltage(
+                    pidController.calculate(
+                        encoder.getPosition(),
+                        currentTargetPosition.getPosition()
+                    ) + (hasAlgae.getAsBoolean() ? algaeHeldFeedForward.calculate(0) : feedForward.calculate(0))
+                );
+            } else {
+                if (hasAlgae.getAsBoolean()) {
+                    elevatorMotor1.setVoltage(
+                        algaeHeldFeedForward.calculate(0)
+                    );
+                } else {
+                    elevatorMotor1.setVoltage(
+                        feedForward.calculate(0)
+                    );
+                }
+            }
+        }
     }
 
     /**
@@ -116,146 +159,34 @@ public class Elevator extends SubsystemBase {
                     speed.getAsDouble()
                 );
             } else {
-                elevatorMotor1.setVoltage(feedForward.calculate(0));
+                if (hasAlgae.getAsBoolean()) {
+                    elevatorMotor1.setVoltage(
+                        algaeHeldFeedForward.calculate(0)
+                    );
+                } else {
+                    elevatorMotor1.setVoltage(
+                        feedForward.calculate(0)
+                    );
+                }
             }
-            
         });
     }
 
-    /**
+    public Command setTargetPosition(ElevatorPositions position) {
-     * A command that will use the feed forward to hold up the elevator.
+        return runOnce(() -> {
-     * Used for feed forward tuning.
+            if (mode == ElevatorControlMode.kManualMaintain) {
-     * 
+                pidController.reset();
-     * @return Sets motor voltage based on feed forward calculation.
+                mode = ElevatorControlMode.kPID;
-     */
+            }
-    public Command maintainPosition() {
 
-        return startRun(() -> {
+            currentTargetPosition = position;
-            
-        //pidController.reset();
-       // pidController.setSetpoint(encoder.getPosition());
-        }, 
-        () -> {
-            /*if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition()
-                    ) + feedForward.calculate(0)
-                );
-            } else {
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            }*/
-
-            elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
         });
-        
-            
-            
-            
-            /*
-            elevatorMotor1.setVoltage(
-                pidController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0)
-            );
-*/
-            /* 
-            if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(0)
-                );
-            } else {
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            }
-             
-        });*/
     }
 
-    /**
+    public boolean atSetpoint() {
-     * Moves the elevator to a target destination (setpoint). 
+        return pidController.atSetpoint();
-     * 
-     * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
-     * @return Sets motor voltage to achieve the target destination
-     */
-    public Command goToSetpoint(DoubleSupplier setpoint) {
-
-        return startRun(() -> {
-        
-        pidController.setSetpoint(setpoint.getAsDouble());
-        pidController.reset();
-        }, 
-        () -> {
-            if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        setpoint.getAsDouble()
-                    ) + feedForward.calculate(0)
-                );
-            } else {
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            }
-        }).until(() -> pidController.atSetpoint());
-        
-            
-            
-            
-            /*
-            elevatorMotor1.setVoltage(
-                pidController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0)
-            );
-*/
-            /* 
-            if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(0)
-                );
-            } else {
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            }
-             
-        });*/
     }
 
-            /* 
-            if(encoder.getPosition() >= setpoint.getAsDouble()){
-                elevatorMotor1.setVoltage(
-                    pidControllerUp.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
-                );
-            }else if(encoder.getPosition() <= setpoint.getAsDouble()){
-                elevatorMotor1.setVoltage(
-                    pidControllerDown.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
-                );
-            }
-                */
-
     /**
      * Returns the current encoder position
      * 

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 algaePhotoswitch;
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -30,6 +31,7 @@ public class Manipulator extends SubsystemBase {
         );
 
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
+        algaePhotoswitch = new DigitalInput(ManipulatorConstants.kAlgaePhotoswitchID);
     }
 
     /**
@@ -90,4 +92,8 @@ public class Manipulator extends SubsystemBase {
     public boolean getCoralBeamBreak() {
         return coralBeamBreak.get();
     }
+
+    public boolean getAlgaePhotoswitch() {
+        return algaePhotoswitch.get();
+    }
 }

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

@@ -2,6 +2,9 @@ package frc.robot.subsystems.sysid;
 
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Volts;
+
+import java.util.function.BooleanSupplier;
+
 import static edu.wpi.first.units.Units.InchesPerSecond;
 
 import edu.wpi.first.units.measure.MutDistance;
@@ -22,8 +25,8 @@ public class ElevatorSysID extends Elevator {
 
     private SysIdRoutine routine;
 
-    public ElevatorSysID() {
+    public ElevatorSysID(BooleanSupplier hasAlgae) {
-        super();
+        super(hasAlgae);
 
         appliedVoltage = Volts.mutable(0);
 
@@ -38,7 +41,7 @@ public class ElevatorSysID extends Elevator {
                 (log) -> {
                     log.motor("elevatorMotor1")
                         .voltage(appliedVoltage.mut_replace(
-                            elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
+                            getMotor1(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
                             encoder.getPosition(), Inches