Fixed field oriented teleop driving

WindingMotor · Jan 2, 2025 · 43e6557 · 43e6557
1 parent e1a1f5f
commit 43e6557
Show file tree

Hide file tree

Showing 5 changed files with 56 additions and 13 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -91,7 +91,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000005e040000120b000009050000"
+      "guid": "0300000009120000544f000011010000"
     }
   ]
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -32,7 +32,7 @@ public class RobotContainer {
 
 	public RobotContainer() {
 		driverController = new CommandXboxController(0); // port 0
-		globalInputMap = InputConstants.XBOX; // Set the global input map to Xbox Controller
+		globalInputMap = InputConstants.TX16S_MAIN; // Set the global input map to Xbox Controller
 
 		vision = new SUB_Vision(Robot.isSimulation() ? new IO_VisionSim() : new IO_VisionReal());
 

diff --git a/src/main/java/frc/robot/commands/CMD_DriveAlign.java b/src/main/java/frc/robot/commands/CMD_DriveAlign.java
@@ -21,6 +21,11 @@ public class CMD_DriveAlign extends Command {
 	private final CommandXboxController controller;
 	private final InputConstants controllerMap;
 
+	// Values > 1 increase sensitivity
+	// Values closer to 1 maintain original sensitivity
+	// Values < 1 decrease sensitivity
+	private final double ROTATION_SENSITIVITY = 0.5;
+
 	public CMD_DriveAlign(
 			SUB_Swerve swerve, CommandXboxController controller, InputConstants controllerMap) {
 		this.swerve = swerve;
@@ -36,6 +41,7 @@ public void initialize() {
 
 	@Override
 	public void execute() {
+
 		// Get joystick inputs with deadband applied
 		double xVelocity =
 				-MathUtil.applyDeadband(
@@ -47,10 +53,21 @@ public void execute() {
 						controller.getRawAxis(controllerMap.strafeAxis) * RobotConstants.MAX_SPEED,
 						controllerMap.driveDeadband);
 
-		double rotationVelocity;
+		// Apply forward and strafe inversions from InputConstants
+		xVelocity *= controllerMap.forwardInverted ? -1 : 1;
+		yVelocity *= controllerMap.strafeInverted ? -1 : 1;
 
 		var alliance = DriverStation.getAlliance();
 
+		// Additional alliance-based inversions
+		if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Blue) {
+			// Invert x and y velocities for blue alliance
+			xVelocity *= -1;
+			yVelocity *= -1;
+		}
+
+		double rotationVelocity;
+
 		// Check for alignment controls
 		if (controller.leftBumper().getAsBoolean() && alliance.isPresent()) {
 			// Speaker alignment with left bumper
@@ -72,6 +89,10 @@ public void execute() {
 							controller.getRawAxis(controllerMap.rotationAxis)
 									* swerve.getMaximumAngularVelocity(),
 							controllerMap.driveDeadband);
+
+			// Apply rotation inversion and sensitivity
+			rotationVelocity *= controllerMap.rotationInverted ? -1 : 1;
+			rotationVelocity *= ROTATION_SENSITIVITY;
 		}
 
 		// Create field-relative ChassisSpeeds

diff --git a/src/main/java/frc/robot/constants/BuildConstants.java b/src/main/java/frc/robot/constants/BuildConstants.java
@@ -12,13 +12,13 @@ public final class BuildConstants {
 	public static final String MAVEN_GROUP = "";
 	public static final String MAVEN_NAME = "SwerveDrive2025";
 	public static final String VERSION = "unspecified";
-	public static final int GIT_REVISION = 21;
-	public static final String GIT_SHA = "3a05efc5f15d2bd7e4b529e7a18a577c05ab538f";
-	public static final String GIT_DATE = "2024-12-31 14:53:23 EST";
+	public static final int GIT_REVISION = 22;
+	public static final String GIT_SHA = "e1a1f5fca0958cbe48bdaaa13ff92c6950e12ecc";
+	public static final String GIT_DATE = "2025-01-01 18:13:35 EST";
 	public static final String GIT_BRANCH = "main";
-	public static final String BUILD_DATE = "2025-01-01 18:06:45 EST";
-	public static final long BUILD_UNIX_TIME = 1735772805306L;
-	public static final int DIRTY = 0;
+	public static final String BUILD_DATE = "2025-01-01 19:50:15 EST";
+	public static final long BUILD_UNIX_TIME = 1735779015486L;
+	public static final int DIRTY = 1;
 
 	private BuildConstants() {}
 }
diff --git a/src/main/java/frc/robot/constants/InputConstants.java b/src/main/java/frc/robot/constants/InputConstants.java
@@ -12,30 +12,52 @@ public enum InputConstants {
 			4, // Forward/Back Axis
 			3, // Left/Right Axis
 			0, // Rotation Axis
-			0.2 // Deadband
+			0.2, // Deadband
+			false, // Forward Inverted
+			false, // Left/Right Inverted
+			false // Rotation Inverted
 			),
 	TX16S_MAIN(
 			1, // Forward/Back Axis
 			0, // Left/Right Axis
 			3, // Rotation Axis
-			0.1 // Deadband
+			0.1, // Deadband
+			false, // Forward Inverted
+			true, // Left/Right Inverted
+			false // Rotation Inverted
 			),
 	KEYBOARD(
 			0, // Forward/Back Axis
 			1, // Left/Right Axis
 			2, // Rotation Axis
-			0.1 // Deadband
+			0.1, // Deadband
+			false, // Forward Inverted
+			false, // Left/Right Inverted
+			false // Rotation Inverted
 			);
 
 	public final int forwardAxis;
+	public final boolean forwardInverted;
 	public final int strafeAxis;
+	public final boolean strafeInverted;
 	public final int rotationAxis;
+	public final boolean rotationInverted;
 	public final double driveDeadband;
 
-	InputConstants(int forwardAxis, int strafeAxis, int rotationAxis, double driveDeadband) {
+	InputConstants(
+			int forwardAxis,
+			int strafeAxis,
+			int rotationAxis,
+			double driveDeadband,
+			boolean forwardInverted,
+			boolean strafeInverted,
+			boolean rotationInverted) {
 		this.forwardAxis = forwardAxis;
 		this.strafeAxis = strafeAxis;
 		this.rotationAxis = rotationAxis;
 		this.driveDeadband = driveDeadband;
+		this.forwardInverted = forwardInverted;
+		this.strafeInverted = strafeInverted;
+		this.rotationInverted = rotationInverted;
 	}
 }