From c4b4f41fbbb5ac74d32d58a2a29a9388166e4489 Mon Sep 17 00:00:00 2001 From: Craftzman7 Date: Wed, 15 Jan 2025 07:54:03 -0800 Subject: [PATCH 1/3] fix: correctly specify chassis speeds --- .../frc2025/subsystems/drivetrain/Drivetrain.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 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 index 472aa3a..da4eec8 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt @@ -265,8 +265,8 @@ object Drivetrain : Subsystem, Sendable { desiredModuleStates = BRAKE_POSITION } else { desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + -translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + -translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, -rotationInput.y * TAU * ROTATION_SENSITIVITY, inputs.gyroRotation ) @@ -311,8 +311,8 @@ object Drivetrain : Subsystem, Sendable { ) desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + -translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + -translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, -magnitude, inputs.gyroRotation ) From 1ba70385eb419c37ca8951eb51a074891fddece5 Mon Sep 17 00:00:00 2001 From: doinkythederp Date: Wed, 15 Jan 2025 14:40:11 -0800 Subject: [PATCH 2/3] fix(Drivetrain): use field-relative chassis speeds --- .../subsystems/drivetrain/Drivetrain.kt | 18 +++++++------- .../com/frcteam3636/frc2025/utils/Inputs.kt | 24 +++++++++++++++++-- 2 files changed, 31 insertions(+), 11 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 index da4eec8..f20608c 100644 --- 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 @@ -265,9 +266,9 @@ object Drivetrain : Subsystem, Sendable { desiredModuleStates = BRAKE_POSITION } else { desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - -translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - -translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - -rotationInput.y * TAU * ROTATION_SENSITIVITY, + translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_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") @@ -311,8 +312,8 @@ object Drivetrain : Subsystem, Sendable { ) desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - -translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, - -translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, -magnitude, inputs.gyroRotation ) @@ -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 index 997dcf3..7b41682 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt @@ -1,7 +1,27 @@ package com.frcteam3636.frc2025.utils 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 + // Joystick x/y are inverted from the standard coordinate system+ + return when (DriverStation.getAlliance().getOrNull()) { + Alliance.Red -> -base + else -> base + } + } + +/** + * Returns the translation of the joystick input. + */ +val Joystick.translation2d: Translation2d // Joystick x/y are inverted from the standard coordinate system + get() = Translation2d(-y, -x) From b4ecec276532a7f2283f83d8d31b885ec00d2e11 Mon Sep 17 00:00:00 2001 From: doinkythederp Date: Wed, 15 Jan 2025 14:48:50 -0800 Subject: [PATCH 3/3] docs: explain inversion --- .../com/frcteam3636/frc2025/utils/Inputs.kt | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt b/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt index 7b41682..547728c 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/utils/Inputs.kt @@ -1,5 +1,6 @@ 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 @@ -13,7 +14,6 @@ import kotlin.jvm.optionals.getOrNull val Joystick.fieldRelativeTranslation2d: Translation2d get() { val base = translation2d - // Joystick x/y are inverted from the standard coordinate system+ return when (DriverStation.getAlliance().getOrNull()) { Alliance.Red -> -base else -> base @@ -23,5 +23,15 @@ val Joystick.fieldRelativeTranslation2d: Translation2d /** * Returns the translation of the joystick input. */ -val Joystick.translation2d: Translation2d // Joystick x/y are inverted from the standard coordinate system - get() = Translation2d(-y, -x) +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 + )