fix: correctly specify chassis speeds (#40)

FRC3636 · Jan 15, 2025 · 232c996 · 232c996
2 parents c43970e + b4ecec2
commit 232c996
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 7 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt
@@ -10,6 +10,7 @@ import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.ROTATI
 import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.ROTATION_SENSITIVITY
 import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.TRANSLATION_SENSITIVITY
 import com.frcteam3636.frc2025.utils.ElasticWidgets
+import com.frcteam3636.frc2025.utils.fieldRelativeTranslation2d
 import com.frcteam3636.frc2025.utils.math.PIDController
 import com.frcteam3636.frc2025.utils.math.PIDGains
 import com.frcteam3636.frc2025.utils.math.TAU
@@ -267,7 +268,7 @@ object Drivetrain : Subsystem, Sendable {
             desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
                 translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY,
                 translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY,
-                -rotationInput.y * TAU * ROTATION_SENSITIVITY,
+                rotationInput.y * TAU * ROTATION_SENSITIVITY,
                 inputs.gyroRotation
             )
         }
@@ -276,7 +277,7 @@ object Drivetrain : Subsystem, Sendable {
     fun driveWithJoysticks(translationJoystick: Joystick, rotationJoystick: Joystick): Command =
         run {
             // Directly accessing Joystick.x/y gives inverted values - use a `Translation2d` instead.
-            drive(translationJoystick.translation2d, rotationJoystick.translation2d)
+            drive(translationJoystick.fieldRelativeTranslation2d, rotationJoystick.translation2d)
         }
 
     @Suppress("unused")
@@ -325,13 +326,12 @@ object Drivetrain : Subsystem, Sendable {
     fun zeroGyro() {
         // Tell the gyro that the robot is facing the other alliance.
         val zeroPos = when (DriverStation.getAlliance().getOrNull()) {
-            DriverStation.Alliance.Blue -> Rotation2d.kZero
-            else -> Rotation2d.k180deg
+            DriverStation.Alliance.Red -> Rotation2d.k180deg
+            else -> Rotation2d.kZero
         }
         io.setGyro(zeroPos)
     }
 
-
     internal object Constants {
         // Translation/rotation coefficient for teleoperated driver controls
         /** Unit: Percent of max robot speed */

diff --git a/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt b/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt
@@ -1,7 +1,37 @@
 package com.frcteam3636.frc2025.utils
 
+import com.frcteam3636.frc2025.Robot
 import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.DriverStation.Alliance
 import edu.wpi.first.wpilibj.Joystick
+import kotlin.jvm.optionals.getOrNull
 
-// Joystick x/y are inverted from the standard coordinate system
-val Joystick.translation2d get() = Translation2d(y, x)
+
+/**
+ * Returns the translation of the joystick input, flipped to match the current alliance.
+ */
+val Joystick.fieldRelativeTranslation2d: Translation2d
+    get() {
+        val base = translation2d
+        return when (DriverStation.getAlliance().getOrNull()) {
+            Alliance.Red -> -base
+            else -> base
+        }
+    }
+
+/**
+ * Returns the translation of the joystick input.
+ */
+val Joystick.translation2d: Translation2d
+    // The field-space translation returned by this method is rotated 90 degrees from the joystick's
+    // perspective. (x, y) -> (y, -x) The joystick's Y-axis is also inverted because of our physical
+    // hardware, but this isn't an issue in simulation.
+    get() = Translation2d(
+        if (Robot.model == Robot.Model.SIMULATION) {
+            y
+        } else {
+            -y
+        },
+        -x
+    )