add basic squaring of omega and velocity magnitudes

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'

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);
+}

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;
+    }
+}