From 6e18bd63dc3c91ad0c38f1184fcb62b577013f16 Mon Sep 17 00:00:00 2001
From: Jack Lingle <captainjack0818@gmail.com>
Date: Mon, 11 Nov 2024 19:41:44 -0500
Subject: [PATCH] add basic squaring of omega and velocity magnitudes

---
 .../java/coppercore/{ => math}/Deadband.java  |  0
 wpi_interface/build.gradle                    |  5 ++
 .../wpi_interface/DriveTemplate.java          |  7 +++
 .../wpi_interface/DriveWithJoysticks.java     | 47 +++++++++++++++++++
 4 files changed, 59 insertions(+)
 rename math/src/main/java/coppercore/{ => math}/Deadband.java (100%)
 create mode 100644 wpi_interface/src/main/java/coppercore/wpi_interface/DriveTemplate.java
 create mode 100644 wpi_interface/src/main/java/coppercore/wpi_interface/DriveWithJoysticks.java

diff --git a/math/src/main/java/coppercore/Deadband.java b/math/src/main/java/coppercore/math/Deadband.java
similarity index 100%
rename from math/src/main/java/coppercore/Deadband.java
rename to math/src/main/java/coppercore/math/Deadband.java
diff --git a/wpi_interface/build.gradle b/wpi_interface/build.gradle
index 1d47afe..79b3c11 100644
--- a/wpi_interface/build.gradle
+++ b/wpi_interface/build.gradle
@@ -20,6 +20,11 @@ def includeDesktopSupport = true
 dependencies {
 	implementation 'edu.wpi.first.wpilibj:wpilibj-java:2024.3.2'
 	implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.3.2'
+	implementation 'edu.wpi.first.wpiutil:wpiutil-java:2024.3.2'
+	implementation 'edu.wpi.first.wpiunits:wpiunits-java:2024.3.2'
+	implementation 'edu.wpi.first.wpimath:wpimath-java:2024.3.2'
+
+	implementation project(":math")
 
 	testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
 	testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
diff --git a/wpi_interface/src/main/java/coppercore/wpi_interface/DriveTemplate.java b/wpi_interface/src/main/java/coppercore/wpi_interface/DriveTemplate.java
new file mode 100644
index 0000000..d4df60f
--- /dev/null
+++ b/wpi_interface/src/main/java/coppercore/wpi_interface/DriveTemplate.java
@@ -0,0 +1,7 @@
+package coppercore.wpi_interface;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+public interface DriveTemplate {
+    public void setGoalSpeeds(ChassisSpeeds goalSpeeds, boolean fieldCentric);
+}
diff --git a/wpi_interface/src/main/java/coppercore/wpi_interface/DriveWithJoysticks.java b/wpi_interface/src/main/java/coppercore/wpi_interface/DriveWithJoysticks.java
new file mode 100644
index 0000000..11fd25e
--- /dev/null
+++ b/wpi_interface/src/main/java/coppercore/wpi_interface/DriveWithJoysticks.java
@@ -0,0 +1,47 @@
+package coppercore.wpi_interface;
+
+import coppercore.math.Deadband;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
+
+public class DriveWithJoysticks extends Command {
+    private DriveTemplate drive;
+    private CommandJoystick leftJoystick;
+    private CommandJoystick rightJoystick;
+
+    public DriveWithJoysticks(
+            DriveTemplate drive, CommandJoystick leftJoystick, CommandJoystick rightJoystick) {
+        this.drive = drive;
+        this.leftJoystick = leftJoystick;
+        this.rightJoystick = rightJoystick;
+    }
+
+    @Override
+    public void execute() {
+        Translation2d linearSpeeds = getLinearVelocity(leftJoystick.getX(), leftJoystick.getY());
+
+        double omega = Deadband.oneAxisDeadband(rightJoystick.getX(), 0.1);
+        omega = Math.copySign(omega * omega, omega);
+
+        drive.setGoalSpeeds(new ChassisSpeeds(linearSpeeds.getX(), linearSpeeds.getY(), omega), true);
+    }
+
+    public Translation2d getLinearVelocity(double x, double y) {
+        double deadband = Deadband.oneAxisDeadband(Math.hypot(x, y), 0.1);
+
+        Rotation2d direction = new Rotation2d(x, y);
+        double squaredMagnitude = deadband * deadband;
+
+        Translation2d linearVelocity =
+                new Pose2d(new Translation2d(), direction)
+                        .transformBy(new Transform2d(squaredMagnitude, 0.0, new Rotation2d()))
+                        .getTranslation();
+
+        return linearVelocity;
+    }
+}