working on pathplanner

src/main/deploy/pathplanner/autos/fein.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "fein"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/fein.path

@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.600204918039607,
+        "y": 6.374590163938041
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.600204918032786,
+        "y": 7.573360655737705
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.459426229508197,
+        "y": 5.978995901639344
+      },
+      "prevControl": {
+        "x": 5.370491803278689,
+        "y": 5.978995901639344
+      },
+      "nextControl": {
+        "x": 2.995158619919281,
+        "y": 5.978995901639344
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.6543032786885246,
+        "y": 6.254713114754098
+      },
+      "prevControl": {
+        "x": 2.613319672131147,
+        "y": 6.278688524590164
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -8,7 +8,6 @@ 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;
@@ -27,9 +26,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;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
 	private ClimberPivot climberPivot;
@@ -56,14 +53,15 @@ public class RobotContainer {
 
 		climberRollers = new ClimberRollers();
 
-		manipulator = new Manipulator();
-
-		//vision = new Vision(drivetrain::getGyroValue);
 		drivetrain = new Drivetrain();
 
-		elevator = new Elevator(manipulator::getAlgaePhotoswitch);
+		vision = new Vision(drivetrain::getGyroValue);
+
+		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
 
+		manipulator = new Manipulator();
+
 		manipulatorPivot = new ManipulatorPivot();
 
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
@@ -73,6 +71,7 @@ public class RobotContainer {
 
 		configureButtonBindings();
 		//elevatorSysIDBindings();
+		//elevatorOnlyBindings();
 
 		configureNamedCommands();
 
@@ -88,6 +87,13 @@ public class RobotContainer {
 		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
 	}*/
 
+	private void elevatorOnlyBindings(){
+		elevator.setDefaultCommand(elevator.maintainPosition());
+		manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());
+
+		driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
+	}
+
 	private void configureButtonBindings() {
 		//Default commands
 		climberPivot.setDefaultCommand(
@@ -107,6 +113,11 @@ public class RobotContainer {
 			)
 		);
 		
+		elevator.setDefaultCommand(
+			elevator.maintainPosition()
+		);
+		
+		
 		manipulatorPivot.setDefaultCommand(
 			manipulatorPivot.maintainPosition()
 		);
@@ -114,7 +125,7 @@ public class RobotContainer {
 
 		manipulator.setDefaultCommand(
 			manipulator.runUntilCollected(
-				() -> 0
+				() -> 0.0
 			)
 		);
 
@@ -125,73 +136,75 @@ public class RobotContainer {
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(() -> 1, true)
+			manipulator.runManipulator(() -> 0.35, true)
 		);
 
 		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.75)
+			manipulator.runUntilCollected(() -> 0.35)
+		);
+
+		driver.leftBumper().whileTrue(
+			manipulator.runUntilCollected(() -> 0.5)
+		);
+
+		driver.rightBumper().whileTrue(
+			manipulator.runManipulator(() -> 1, false)
 		);
 
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
+
+		driver.y().whileTrue(drivetrain.zeroHeading());
 		
 		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
 		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
 
 		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
+		driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false));
 		
 		//Operator inputs
 		operator.povUp().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL4, 
+				ElevatorConstants.kL4Position, 
 				ManipulatorPivotConstants.kL4Position
 			)
 		);
 
 		operator.povRight().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL3, 
+				ElevatorConstants.kL3Position, 
 				ManipulatorPivotConstants.kL3Position
 			)
 		);
 
 		operator.povLeft().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL2, 
+				ElevatorConstants.kL2Position, 
 				ManipulatorPivotConstants.kL2Position
 			)
 		);
 
 		operator.povDown().onTrue(
 			safeMoveManipulator(
-				ElevatorPositions.kL1, 
+				ElevatorConstants.kL1Position, 
 				ManipulatorPivotConstants.kL1Position
 			)
 		);
 
 		operator.a().onTrue(
-			coralIntakeRoutine()
+			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
 		);
 
 		operator.x().onTrue(
-			algaeIntakeRoutine(true)
+			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
 		);
 
 		operator.b().onTrue(
-			algaeIntakeRoutine(false)
+			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
 		);
 
-		operator.y().onTrue(
-			elevator.setTargetPosition(ElevatorPositions.kL2)
-		);
-
-		new Trigger(() -> Math.abs(MathUtil.applyDeadband(operator.getLeftY(), .05)) > .05).onTrue(
-			elevator.runManualElevator(operator::getLeftY)
-		);
-	
-
 	}
 
 	private void configureNamedCommands() {
@@ -246,12 +259,22 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
 			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint)
+		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
 			.withSize(1, 1)
 			.withPosition(5, 0)
 			.withWidget(BuiltInWidgets.kTextView);
 
-		sensorTab.addDouble("Elevator error", elevator::getPIDError)
+		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
+			.withSize(1, 1)
+			.withPosition(5, 1)
+			.withWidget(BuiltInWidgets.kTextView);
+
+			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
+			.withSize(1, 1)
+			.withPosition(5, 0)
+			.withWidget(BuiltInWidgets.kTextView);
+
+		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
 			.withSize(1, 1)
 			.withPosition(5, 1)
 			.withWidget(BuiltInWidgets.kTextView);
@@ -260,6 +283,9 @@ public class RobotContainer {
 
 		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
 
+		sensorTab.addDouble("dt distance", drivetrain::driveDistance);
+
+
 
  	}
 
@@ -274,7 +300,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command coralIntakeRoutine() {
 		return moveManipulator(
-			ElevatorPositions.kCoralIntake, 
+			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
 			.andThen(manipulator.runUntilCollected(() -> .5));
@@ -289,7 +315,7 @@ public class RobotContainer {
 	@SuppressWarnings("unused")
 	private Command algaeIntakeRoutine(boolean l2) {
 		return moveManipulator(
-			l2 ? ElevatorPositions.kL2Algae : ElevatorPositions.kL3Algae, 
+			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
 			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
 			.andThen(manipulator.runUntilCollected(() -> 1));
@@ -302,10 +328,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(ElevatorPositions elevatorPosition, double armPosition) {
+	private Command moveManipulator(double 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.getPosition())) || (manipulatorPivot.isMotionSafe() && manipulatorPivot.isMotionSafe(armPosition))) { 
+		if ((elevator.isMotionSafe() && elevator.isMotionSafe(elevatorPosition)) || (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
@@ -334,7 +360,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(ElevatorPositions elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
+	private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
 		
 		/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
 			armPosition = MathUtil.clamp(
@@ -346,16 +372,13 @@ public class RobotContainer {
 		
 		return Commands.either(
 			Commands.either(
-				elevator.setTargetPosition(elevatorPosition).andThen(
-					new WaitUntilCommand(elevator::atSetpoint),
-					manipulatorPivot.goToSetpoint(() -> armPosition)
-				),
-				elevator.setTargetPosition(elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
+				elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
 				() -> sequential
 			), 
 			Commands.either(
-				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.setTargetPosition(elevatorPosition)), 
-				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.setTargetPosition(elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)), 
+				manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)), 
 				() -> sequential
 			), 
 			() -> elevatorFirst
@@ -363,24 +386,18 @@ public class RobotContainer {
 	}
 
 	@SuppressWarnings("unused")
-	private Command manipulatorSafeTravel(ElevatorPositions elevatorPosition, double armPosition, boolean isL4){
+	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
 		if(!isL4){
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
-				elevator.setTargetPosition(elevatorPosition),
-				new WaitUntilCommand(elevator::atSetpoint),
+				elevator.goToSetpoint(() -> elevatorPosition),
 				manipulatorPivot.goToSetpoint(() -> armPosition));
 			
 		}else{
 			return Commands.sequence(
 				manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
-				elevator.setTargetPosition(elevatorPosition),
-				new WaitUntilCommand(() -> elevator.getEncoderPosition() > ElevatorPositions.kL4Transition.getPosition()),
-				Commands.parallel(
-					manipulatorPivot.goToSetpoint(() -> armPosition), 
-					elevator.setTargetPosition(elevatorPosition).andThen(new WaitUntilCommand(elevator::atSetpoint))
-				)
-			);
+				elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
 		}
 		
 	}
@@ -392,15 +409,18 @@ public class RobotContainer {
 	 * @param armPosition The target rotation of the arm
 	 * @return Moves the elevator and arm to the setpoints
 	 */
-	@SuppressWarnings("unused")
-	private Command safeMoveManipulator(ElevatorPositions elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
-			.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
+	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
 	}
 
 	@SuppressWarnings("unused")
 	private Command startingConfig() {
-		return moveManipulatorUtil(ElevatorPositions.kStartingPosition, 0, false, true)
+		return moveManipulatorUtil(0, 0, false, true)
 			.alongWith(climberPivot.climb(ClimberPivotConstants.kClimberStartingPosition, .1));
 	}
 }

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

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.8;
+    public static final double kMaxSpeedMetersPerSecond = 5.5;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration

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

@@ -11,34 +11,6 @@ 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;
 
@@ -64,16 +36,23 @@ 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 = 8;
+    public static final double kL3Position = 25.0;
+    public static final double kL4Position = 50.5;
+    public static final double kL4TransitionPosition = 40.0;
+    public static final double kL2AlgaePosition = 21.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
+    public static final double kMaxHeight = 51.0; //actual is 51
+
+    public static final double kVoltageLimit = 7;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;

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

@@ -4,9 +4,7 @@ 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/constants/ManipulatorPivotConstants.java

@@ -34,19 +34,19 @@ public class ManipulatorPivotConstants {
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0.7815;
+    public static final double kEncoderOffset = 0.780;
 
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
-    public static final double kL2Position = Units.degreesToRadians(60.0);
-    public static final double kL3Position = Units.degreesToRadians(60.0);
+    public static final double kL2Position = Units.degreesToRadians(22.0);
+    public static final double kL3Position = Units.degreesToRadians(22.0);
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kProcesserPosition = Units.degreesToRadians(175.0);
     public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
     /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 

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

@@ -22,7 +22,7 @@ public class ModuleConstants {
     public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
     // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
     // teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
+    public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 

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

@@ -1,5 +1,5 @@
 package frc.robot.constants;
 
 public class NeoMotorConstants {
-    public static final double kFreeSpeedRpm = 5676;
+    public static final double kFreeSpeedRpm = 6000; //for kraken not neo
 }

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

@@ -248,9 +248,12 @@ public class Drivetrain extends SubsystemBase {
         m_rearRight.resetEncoders();
     }
 
-    /** Zeroes the heading of the robot. */
-    public void zeroHeading() {
-        gyro.reset();
+    /** Zeroes the heading of the robot. 
+     * @return */
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+        });
     }
 
     public double getGyroValue() {
@@ -278,4 +281,8 @@ public class Drivetrain extends SubsystemBase {
     public void addVisionMeasurement(Pose2d pose, double timestamp){
         m_estimator.addVisionMeasurement(pose, timestamp);
     }
+
+    public double driveDistance(){
+        return m_frontLeft.getTotalDist();
+    }
 }

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

@@ -1,6 +1,5 @@
 package frc.robot.subsystems;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
@@ -16,8 +15,6 @@ 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;
@@ -25,20 +22,14 @@ public class Elevator extends SubsystemBase {
 
     protected RelativeEncoder encoder;
 
-    private BooleanSupplier hasAlgae;
-
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidController;
+    private PIDController pidControllerUp;
+    private PIDController pidControllerDown;
 
     private ElevatorFeedforward feedForward;
-    private ElevatorFeedforward algaeHeldFeedForward;
-
-    private ElevatorControlMode mode;
-
-    private ElevatorPositions currentTargetPosition;
     
-    public Elevator(BooleanSupplier hasAlgae) {
+    public Elevator() {
         elevatorMotor1 = new SparkMax(
             ElevatorConstants.kElevatorMotor1ID, 
             MotorType.kBrushless
@@ -63,36 +54,35 @@ public class Elevator extends SubsystemBase {
 
         encoder = elevatorMotor1.getEncoder();
 
-        this.hasAlgae = hasAlgae;
-
         bottomLimitSwitch = new DigitalInput(
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidController = new PIDController(
+        pidControllerDown = new PIDController(
             ElevatorConstants.kDownControllerP, 
             ElevatorConstants.kDownControllerI,
             ElevatorConstants.kDownControllerD
         );
-        pidController.setSetpoint(0);
+        pidControllerDown.setSetpoint(0);
 
-        pidController.setTolerance(ElevatorConstants.kAllowedError);
+        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
+
+        pidControllerUp = new PIDController(
+            ElevatorConstants.kUpControllerP, 
+            ElevatorConstants.kUpControllerI,
+            ElevatorConstants.kUpControllerD
+        );
+        pidControllerUp.setSetpoint(0);
+
+        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
+
+        
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
-
-        algaeHeldFeedForward = new ElevatorFeedforward(
-            ElevatorConstants.kFeedForwardAlgaeHeldS, 
-            ElevatorConstants.kFeedForwardAlgaeHeldG, 
-            ElevatorConstants.kFeedForwardAlgaeHeldV
-        );
-
-        mode = ElevatorControlMode.kPID;
-
-        currentTargetPosition = ElevatorPositions.kCoralIntake;
     }
 
     @Override
@@ -100,27 +90,6 @@ 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)
-                    );
-                }
-            }
-        }
     }
 
     /**
@@ -151,10 +120,7 @@ public class Elevator extends SubsystemBase {
      * @return Sets motor voltage to translate the elevator and maintain position
      */
     public Command runManualElevator(DoubleSupplier speed) {
-        return startRun(() -> {
-            mode = ElevatorControlMode.kManualMaintain;
-        },
-        () -> {
+        return run(() -> {
             double desired = speed.getAsDouble();
 
             if(Math.abs(MathUtil.applyDeadband(desired, .05)) > 0) {
@@ -162,33 +128,158 @@ public class Elevator extends SubsystemBase {
                     speed.getAsDouble()
                 );
             } else {
-                if (hasAlgae.getAsBoolean()) {
-                    elevatorMotor1.setVoltage(
-                        algaeHeldFeedForward.calculate(0)
-                    );
-                } else {
-                    elevatorMotor1.setVoltage(
-                        feedForward.calculate(0)
-                    );
-                }
+                elevatorMotor1.setVoltage(feedForward.calculate(0));
             }
+            
         });
     }
 
-    public Command setTargetPosition(ElevatorPositions position) {
-        return runOnce(() -> {
-            if (mode == ElevatorControlMode.kManualMaintain) {
-                pidController.reset();
-                mode = ElevatorControlMode.kPID;
-            }
+    /**
+     * A command that will use the feed forward to hold up the elevator.
+     * Used for feed forward tuning.
+     * 
+     * @return Sets motor voltage based on feed forward calculation.
+     */
+    public Command maintainPosition() {
+
+        return startRun(() -> {
+/* 
+            pidControllerUp.reset();
+            pidControllerDown.reset();
+            */
+        }, 
+        () -> {
+/* 
+            double upOutput = pidControllerUp.calculate(getEncoderPosition());
+            double downOutput = pidControllerDown.calculate(getEncoderPosition());
+
+            if(pidControllerUp.getSetpoint()>encoder.getPosition()) 
+                elevatorMotor1.setVoltage( MathUtil.clamp(
+                    upOutput + feedForward.calculate(0), -1, 1)
+                );
+            else{
+                elevatorMotor1.setVoltage(
+                    MathUtil.clamp(
+                    downOutput + feedForward.calculate(0), -1, 1)
+                );
+            } 
+            */
+
+            elevatorMotor1.setVoltage(
+                    feedForward.calculate(0)
+                );
 
-            currentTargetPosition = position;
         });
+        
     }
 
-    public boolean atSetpoint() {
-        return pidController.atSetpoint();
+    /**
+     * Moves the elevator to a target destination (setpoint). 
+     * 
+     * @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) {
+            
+        if (setpoint.getAsDouble() == 0) {
+            return startRun(() -> {
+        
+                pidControllerUp.reset();
+                pidControllerDown.reset();
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+                
+                }, 
+                () -> {
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+        
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
+                    .andThen(runManualElevator(() -> -.1)
+                    .until(() -> encoder.getPosition() == 0));
+
+        } else {
+            return startRun(() -> {
+        
+                pidControllerUp.reset();
+                pidControllerDown.reset();
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+        
+                },
+                () -> {
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+        
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.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
@@ -227,11 +318,19 @@ public class Elevator extends SubsystemBase {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 
-    public double getPIDSetpoint() {
-        return pidController.getSetpoint();
+    public double getPIDUpSetpoint() {
+        return pidControllerUp.getSetpoint();
     }
 
-    public double getPIDError() {
-        return pidController.getError();
+    public double getPIDUpError() {
+        return pidControllerUp.getError();
+    }
+
+    public double getPIDDownSetpoint() {
+        return pidControllerDown.getSetpoint();
+    }
+
+    public double getPIDDownError() {
+        return pidControllerDown.getError();
     }
 }

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

@@ -138,4 +138,8 @@ public class MAXSwerveModule {
     public void resetEncoders() {
         m_drive.setPosition(0);
     }
+
+    public double getTotalDist(){
+        return m_drive.getPosition().getValueAsDouble();
+    }
 }

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

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

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

@@ -40,9 +40,9 @@ public class Vision{
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
         NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
 
-        blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
-        blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
-        blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+        blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
+        blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
+        blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
 
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
         blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
@@ -59,24 +59,23 @@ public class Vision{
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
     }
     
-    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
         Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
             VisionConstants.globalTagCoords[tagID][1],
             new Rotation2d());
 
-        Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
+        Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
 
         Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
 
         Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
 
-        return new Pose2d(globalPose.getTranslation(), gyroAngle);
+        return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
     }
 
     public Pose2d getBlackGlobalPose(){
         return relativeToGlobalPose2d(getBlackClosestTag(), 
-            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
-            new Rotation2d(gyroAngle.getAsDouble()));
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
     }
 
     public double getBlackRelativeX(){
@@ -109,8 +108,7 @@ public class Vision{
 
     public Pose2d getOrangeGlobalPose(){
         return relativeToGlobalPose2d(getOrangeClosestTag(), 
-            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), 
-            new Rotation2d(gyroAngle.getAsDouble()));
+            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
     }
     
     public double getOrangeRelativeX(){

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

@@ -2,9 +2,6 @@ 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;
@@ -25,8 +22,8 @@ public class ElevatorSysID extends Elevator {
 
     private SysIdRoutine routine;
 
-    public ElevatorSysID(BooleanSupplier hasAlgae) {
-        super(hasAlgae);
+    public ElevatorSysID() {
+        super();
 
         appliedVoltage = Volts.mutable(0);
 
@@ -41,7 +38,7 @@ public class ElevatorSysID extends Elevator {
                 (log) -> {
                     log.motor("elevatorMotor1")
                         .voltage(appliedVoltage.mut_replace(
-                            getMotor1(), Volts
+                            elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts
                         ))
                         .linearPosition(elevatorPosition.mut_replace(
                             encoder.getPosition(), Inches