maybe maybe maybe

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

@@ -279,15 +279,15 @@ void loadCommands() {
                     driveSubsystem::getRotation
     ));
 
-    DoubleSupplier dPadX = () -> {
+    DoubleSupplier dPadY = () -> {
       if (driveController.povLeft().getAsBoolean()) {
-        return -1.0;
-      } else if (driveController.povRight().getAsBoolean()) {
         return 1.0;
+      } else if (driveController.povRight().getAsBoolean()) {
+        return -1.0;
       } else { return 0.0;}
     };
 
-    DoubleSupplier dPadY = () -> {
+    DoubleSupplier dPadX = () -> {
       if (driveController.povDown().getAsBoolean()) {
         return -1.0;
       } else if (driveController.povUp().getAsBoolean()) {

src/main/java/frc/robot/commands/DriveCommands/FieldDriveElevatorLimitedCommand.java

@@ -0,0 +1,108 @@
+package frc.robot.commands.DriveCommands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.OperatorInput;
+import frc.robot.constants.DriveConstants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
+
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+public class FieldDriveElevatorLimitedCommand extends Command {
+    private final DriveSubsystem drive;
+    private final DoubleSupplier xSupplier;
+    private final DoubleSupplier ySupplier;
+    private final DoubleSupplier omegaSupplier;
+    private final ElevatorSubsystem elevator;
+    private final Supplier<Rotation2d> headingSupplier;
+
+    public FieldDriveElevatorLimitedCommand(DriveSubsystem drive, ElevatorSubsystem elevatorSubsystem, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier, Supplier<Rotation2d> headingSupplier) {
+        this.drive = drive;
+        this.xSupplier = xSupplier;
+        this.ySupplier = ySupplier;
+        this.omegaSupplier = omegaSupplier;
+        this.headingSupplier = headingSupplier;
+        this.elevator = elevatorSubsystem;
+    }
+
+
+    public void execute() {
+        // Apply deadband
+        double DEADBAND = 0.1;
+        double linearMagnitude =
+                MathUtil.applyDeadband(
+                        Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
+        Rotation2d linearDirection =
+                new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+        double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);
+
+        // Square values
+        linearMagnitude = linearMagnitude * linearMagnitude;
+        omega = Math.copySign(omega * omega, omega);
+
+        // Calcaulate new linear velocity
+        Translation2d linearVelocity =
+                new Pose2d(new Translation2d(), linearDirection)
+                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+                        .getTranslation();
+
+        // Convert to field relative speeds & send command
+        boolean isFlipped =
+                DriverStation.getAlliance().isPresent()
+                        && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
+
+
+        double speedFactor;
+        if (OperatorInput.slowModeHold.getAsBoolean()) {
+            speedFactor = DriveConstants.slowSpeed;
+        } else if (OperatorInput.turboModeHold.getAsBoolean()) {
+            speedFactor = DriveConstants.turboSpeed;
+        } else {
+            speedFactor = DriveConstants.normalSpeed;
+        }
+
+        speedFactor *= speedRamp(elevator.getLoadHeight(),
+                ElevatorConstants.minHeight + 0.2, ElevatorConstants.maxHeight, 0, 1);
+
+
+        ChassisSpeeds speeds_robotOriented =  new ChassisSpeeds(
+                linearVelocity.getX() * speedFactor, //4.5 is the experimentally determined max velocity
+                linearVelocity.getY() * speedFactor,
+                omega * Math.PI * 1.5
+        );
+
+        Rotation2d headingFlipped = headingSupplier.get();
+
+        if (isFlipped) {
+            headingFlipped = headingFlipped.plus(Rotation2d.fromDegrees(180));
+        }
+
+        drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds_robotOriented, headingFlipped));
+    }
+
+
+    // returns a ramped speed value
+    // a value between an min max, gets mapped to a different linear scale
+    private double speedRamp(double value, double inputMin, double inputMax, double outputMin, double outputMax) {
+        if (!(value <= inputMax && value >= inputMin)) {return 1;}
+        double range1 = inputMax - inputMin;
+        double diff1 = value - inputMin;
+        double firstScalar = diff1 / range1;
+
+        double range2 = outputMax - outputMin;
+        return range2 * firstScalar;
+    };
+
+    public void end(boolean interrupted) {
+        drive.stop();
+    }
+}

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

@@ -13,10 +13,10 @@ public class ArmConstants {
     public static final double kSSim = 0;
     public static double kASim = 0;
 
-    public static double kS = 0;
-    public static double kV = 2;
+    public static double kS = 0.26427;
+    public static double kV = 1.5096;
     public static double kG = 0;
-    public static double kA = 0;
+    public static double kA = 0.33715;
     public static double kP = 0;
     public static double kI = 0;
     public static double kD = 0;

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

@@ -36,7 +36,7 @@ public class DriveConstants {
     public static final double maxSpeedMetersPerSec = 4.8;
     public static final double turboSpeed =  maxSpeedMetersPerSec;
     public static final double normalSpeed = maxSpeedMetersPerSec - 1;
-    public static final double slowSpeed = maxSpeedMetersPerSec - 2;
+    public static final double slowSpeed = maxSpeedMetersPerSec - 3;
     public static final double alignmentSpeed = 1;
 
     public static final double odometryFrequency = 100.0; // Hz

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

@@ -49,7 +49,7 @@ public class ElevatorConstants {
     public static double L4 = 0;
     public static double carriageMass = 20.0;
     public static double maxHeight = 0.571;
-    public static double minHeight = -0.340;
+    public static double minHeight = -0.240;
     public static double maxVelocity = 1;
     public static double maxAcceleration = 1;