safe travels command and constants

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

@@ -95,6 +95,8 @@ public class RobotContainer {
 
 		elevator.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
 
+		manipulatorPivot.setDefaultCommand(moveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+
 		manipulator.setDefaultCommand(
 			manipulator.defaultCommand()
 		);
@@ -295,6 +297,22 @@ public class RobotContainer {
 		);
 	}
 
+	private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
+		if(!isL4){
+			return Commands.sequence(
+				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
+				elevator.goToSetpoint(elevatorPosition),
+				manipulatorPivot.goToSetpoint(armPosition));
+			
+		}else{
+			return Commands.sequence(
+				manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
+				elevator.goToSetpoint(elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
+				Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(elevatorPosition));
+		}
+		
+	}
+
 	/**
 	 * Moves the arm and elevator in a safe way.
 	 * 

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

@@ -45,10 +45,12 @@ public class ElevatorConstants {
     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 kL2AlgaePosition = 0;
+    public static final double kL4TransitionPosition = 40.0;
-    public static final double kL3AlgaePosition = 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 kBracePosition = 5.5;
     public static final double kMaxHeight = 53.0;
 
     // 1, 7, 10 are the defaults for these, change as necessary

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

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Volts;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -15,9 +14,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 
 public class ManipulatorPivotConstants {
     public static final int kArmMotorID = 0;
-    public static final int kCANcoderID = 0;
 
-    public static final int kMotorAmpsMax = 40;
+    public static final int kMotorCurrentMax = 40;
 
     public static final double kPivotConversion = 12/60 * 20/60 * 12/28;
 
@@ -41,9 +39,10 @@ public class ManipulatorPivotConstants {
     public static final double kL1Position = Units.degreesToRadians(0.0);
     public static final double kL2Position = Units.degreesToRadians(25.0);
     public static final double kL3Position = Units.degreesToRadians(60.0);
-    public static final double kL4Position = 0;
+    public static final double kL4Position = Units.degreesToRadians(45.0);
-    public static final double kL2AlgaePosition = 0;
+    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
-    public static final double kL3AlgaePosition = 0;
+    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
+    public static final double kProcesserPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
     public static final double kArmSafeStowPosition = Units.degreesToRadians(60.0);
     /**The forward rotation limit of the arm */
@@ -65,14 +64,12 @@ public class ManipulatorPivotConstants {
         Seconds.of(kSysIDTimeout)
     );
 
-    public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
-
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
 
     static {
 
         motorConfig
-            .smartCurrentLimit(kMotorAmpsMax)
+            .smartCurrentLimit(kMotorCurrentMax)
             .idleMode(kIdleMode);
         motorConfig.absoluteEncoder.positionConversionFactor(kPivotConversion);
         motorConfig.closedLoop