From 612e7864749ef97c77b439978b3ed56358643cad Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Thu, 21 Mar 2024 16:34:54 -0700 Subject: [PATCH] Update everything for using field oriented vectors --- SeriouslyCommonLib | 2 +- .../arm/commands/ArmMaintainerCommand.java | 4 +-- .../drive/commands/PointAtNoteCommand.java | 12 ++----- .../TankDriveWithJoysticksCommand.java | 4 +-- .../drive/PointAtNoteCommandTest.java | 36 ++++--------------- 5 files changed, 15 insertions(+), 43 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 9ae8669f..33c566ba 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 9ae8669f3963f3ec1e9dfb94fcb939f66d01d916 +Subproject commit 33c566bade73574c399597d8ba16b60a6cdb2f61 diff --git a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java index d42ff911..d27f9a21 100644 --- a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java @@ -152,12 +152,12 @@ protected boolean additionalAtGoalChecks() { @Override protected Double getHumanInput() { double fundamentalInput = MathUtils.deadband( - oi.operatorFundamentalsGamepad.getLeftVector().y, + oi.operatorFundamentalsGamepad.getLeftVector().getY(), oi.getOperatorGamepadTypicalDeadband(), (x) -> x); double advancedInput = MathUtils.deadband( - oi.operatorGamepadAdvanced.getLeftVector().y, + oi.operatorGamepadAdvanced.getLeftVector().getY(), oi.getOperatorGamepadTypicalDeadband(), (x) -> x); diff --git a/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java b/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java index 41c6f2c1..7c9a5161 100644 --- a/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java @@ -83,8 +83,7 @@ public void execute() { // if we're very close to the note, stop trying to rotate, it gets wonky var movement = MathUtils.deadband( - getDriveIntent(toNoteTranslation, oi.driverGamepad.getLeftVector(), - DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue)), + getDriveIntent(toNoteTranslation, oi.driverGamepad.getLeftFieldOrientedVector()), oi.getDriverGamepadTypicalDeadband(), (x) -> x); double rotationPower = 0; // if we're far enough away, rotate towards the note (if we're too close, the ) @@ -96,14 +95,9 @@ public void execute() { drive.move(new XYPair(-movement, 0), rotationPower); } - public static double getDriveIntent(Translation2d fieldTranslationToTarget, XYPair driveJoystick, Alliance alliance) { + public static double getDriveIntent(Translation2d fieldTranslationToTarget, Translation2d driveJoystick) { var toNoteVector = fieldTranslationToTarget.toVector().unit(); - var driverVector = VecBuilder.fill(driveJoystick.y, -driveJoystick.x); - if(alliance == DriverStation.Alliance.Red) { - // invert both axis - driverVector = driverVector.div(-1); - } - var dot = toNoteVector.dot(driverVector); + var dot = toNoteVector.dot(driveJoystick.toVector()); return dot; } diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java index 4bfcf466..f33d58bf 100644 --- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java @@ -27,8 +27,8 @@ public void initialize() { @Override public void execute() { driveSubsystem.tankDrive( - MathUtils.deadband(oi.driverGamepad.getLeftVector().y, 0.15), - MathUtils.deadband(oi.driverGamepad.getRightVector().y, 0.15) + MathUtils.deadband(oi.driverGamepad.getLeftVector().getY(), 0.15), + MathUtils.deadband(oi.driverGamepad.getRightVector().getY(), 0.15) ); } } diff --git a/src/test/java/competition/subsystems/drive/PointAtNoteCommandTest.java b/src/test/java/competition/subsystems/drive/PointAtNoteCommandTest.java index 63a0acea..6ae87ce5 100644 --- a/src/test/java/competition/subsystems/drive/PointAtNoteCommandTest.java +++ b/src/test/java/competition/subsystems/drive/PointAtNoteCommandTest.java @@ -14,46 +14,24 @@ public class PointAtNoteCommandTest extends BaseCompetitionTest { @Test - public void testGetDriveIntentBlue() { - var alliance = Alliance.Blue; + public void testGetDriveIntent() { // completely aligned with the direction we want to go // exact y - assertEquals( 1.0, PointAtNoteCommand.getDriveIntent(new Translation2d(0, 5), new XYPair(-1, 0), alliance), 0.001); + assertEquals( 1.0, PointAtNoteCommand.getDriveIntent(new Translation2d(0, 5), new Translation2d(0, 1)), 0.001); // exact x - assertEquals( 1.0, PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new XYPair(0, 1), alliance), 0.001); + assertEquals( 1.0, PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new Translation2d(1, 0)), 0.001); // exact opposite y - assertEquals(-1, PointAtNoteCommand.getDriveIntent(new Translation2d(0, 5), new XYPair(1, 0), alliance), 0.001); + assertEquals(-1, PointAtNoteCommand.getDriveIntent(new Translation2d(0, 5), new Translation2d(0, -1)), 0.001); // exact opposite x - assertEquals(-1, PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new XYPair(0, -1), alliance), 0.001); + assertEquals(-1, PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new Translation2d(-1, 0)), 0.001); // 45 degrees should be half power - assertEquals(0.5, PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new XYPair(0.5, 0.5), alliance), 0.001); + assertEquals(0.5, PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new Translation2d(0.5, 0.5)), 0.001); // small floating joystick number - assertTrue(Math.abs(PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new XYPair(0.01, 0.01), alliance)) < 0.02); + assertTrue(Math.abs(PointAtNoteCommand.getDriveIntent(new Translation2d(5, 0), new Translation2d(0.01, 0.01))) < 0.02); } - @Test - public void testGetDriveIntentRed() { - var alliance = Alliance.Red; - // completely aligned with the direction we want to go - // exact y - assertEquals( 1.0, PointAtNoteCommand.getDriveIntent(new Translation2d(0, -5), new XYPair(-1, 0), alliance), 0.001); - // exact x - assertEquals( 1.0, PointAtNoteCommand.getDriveIntent(new Translation2d(-5, 0), new XYPair(0, 1), alliance), 0.001); - - - // exact opposite y - assertEquals(-1, PointAtNoteCommand.getDriveIntent(new Translation2d(0, -5), new XYPair(1, 0), alliance), 0.001); - // exact opposite x - assertEquals(-1, PointAtNoteCommand.getDriveIntent(new Translation2d(-5, 0), new XYPair(0, -1), alliance), 0.001); - - // 45 degrees should be half power - assertEquals(0.5, PointAtNoteCommand.getDriveIntent(new Translation2d(-5, 0), new XYPair(0.5, 0.5), alliance), 0.001); - - // small floating joystick number - assertTrue(Math.abs(PointAtNoteCommand.getDriveIntent(new Translation2d(-5, 0), new XYPair(0.01, 0.01), alliance)) < 0.02); - } }