fix controller and AutoDriveTo

src/main/java/frc/robot/Constants.java

@@ -48,8 +48,8 @@ public static enum Bot {
 	}
 
 	public static class DriverConstants {
-		public static final int DRIVER_JOYSTICK_PORT = 1;
-		public static final int OPERATOR_JOYSTICK_PORT = 0;
+		public static final int DRIVER_JOYSTICK_PORT = 0;
+		public static final int OPERATOR_JOYSTICK_PORT = 1;
 
 		public static final double DEAD_ZONE = 0.25;
 

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

@@ -6,6 +6,7 @@
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.commands.Autos;
+import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.AutoPosition;
 import frc.robot.commands.SetLightstripColor;
@@ -144,6 +145,9 @@ public void setUpDriveController() {
 			// joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 			// joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates,
 			// drivetrain));
+
+			drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
+
 		} else {
 			SmartDashboard.putString("Drive Ctrl", "GamePad");
 
@@ -165,6 +169,8 @@ public void setUpDriveController() {
 			xbox.povUp().onTrue(Commands.runOnce(inputs::increaseSpeedLevel));
 
 			xbox.a().whileTrue(new AutoPosition(drivetrain, vision));
+
+			drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
 		}
 	}
 

src/main/java/frc/robot/commands/AutoDriveTo.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
 public class AutoDriveTo extends Command {
@@ -56,20 +57,25 @@ public void initialize() {
 	public void execute() {
 		Pose2d position = this.drivetrain.getPosition();
 
-		double x = position.getX() - initX;
-		double y = position.getY() - initY;
+		double targetX = position.getX() - initX;
+		double targetY = position.getY() - initY;
 
-		double xSpeed = xMovePID.calculate(x, goalX);
-		double ySpeed = yMovePID.calculate(y, goalY);
+		double xSpeed = xMovePID.calculate(targetX, goalX);
+		double ySpeed = yMovePID.calculate(targetY, goalY);
 
+		SmartDashboard.putNumber("Speed X", xSpeed);
+		SmartDashboard.putNumber("Speed Y", ySpeed);
 		double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED;
-		double maxSpeed = Math.max(Math.abs(x), Math.abs(y));
+		double maxSpeed = Math.max(Math.abs(xSpeed), Math.abs(ySpeed));
 
 		if (maxSpeed > speedLimit) {
-			xSpeed /= speedLimit * maxSpeed;
-			ySpeed /= speedLimit * maxSpeed;
+			xSpeed = (xSpeed / maxSpeed) * speedLimit;
+			ySpeed = (ySpeed / maxSpeed) * speedLimit;
 		}
 
+		SmartDashboard.putNumber("Scaled X", xSpeed);
+		SmartDashboard.putNumber("Scaled Y", ySpeed);
+
 		drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, 0));
 		drivetrain.updateSmartDashboard();
 	}

src/main/java/frc/robot/commands/AutoPosition.java

@@ -67,7 +67,7 @@ public void initialize() {
 		Rotation2d rot = new Rotation2d(-(Math.PI-angle));
 		Commands.sequence(
 				new AutoRotateTo(drivetrain, rot, false)
-				//,new AutoDriveTo(drivetrain, trans)
+				,new AutoDriveTo(drivetrain, trans)
 				).schedule();
 		;