Add TuneG, TuneS, and TuneV to wpi_interface.feedforward_tuning

wpi_interface/build.gradle

@@ -2,6 +2,7 @@ import java.text.SimpleDateFormat
 
 plugins {
 	id "java"
+	id "java-library"
 	id "maven-publish"
 	id "com.peterabeles.gversion" version "1.10"
 	id "com.diffplug.spotless" version "6.24.0"
@@ -18,9 +19,13 @@ def includeDesktopSupport = true
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
 dependencies {
+	implementation project(":controls")
+
 	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'
+
 	testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
 	testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
@@ -37,15 +42,15 @@ tasks.withType(JavaCompile) {
 	options.compilerArgs.add '-XDstringConcat=inline'
 }
 
-project.compileJava.dependsOn(createVersionFile)
-gversion {
-srcDir       = "src/main/java/"
-classPackage = "frc.robot"
-className    = "BuildConstants"
-dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-timeZone     = "America/New_York"
-indent       = "  "
-}
+// project.compileJava.dependsOn(createVersionFile)
+// gversion {
+// srcDir       = "src/main/java/"
+// classPackage = "frc.robot"
+// className    = "BuildConstants"
+// dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+// timeZone     = "America/New_York"
+// indent       = "  "
+// }
 
 repositories {
 	mavenCentral()

wpi_interface/src/main/java/feedforward_tuning/TuneG.java

@@ -0,0 +1,47 @@
+package coppercore.wpi_interface.feedforward_tuning;
+
+import coppercore.controls.Tunable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class TuneG extends Command {
+    private Tunable subsystem;
+
+    private int slot;
+
+    double startPosition;
+
+    double kG;
+    double kS;
+
+    public TuneG(Tunable subsystem, int slot) {
+        this.subsystem = subsystem;
+        this.kS = SmartDashboard.getNumber("Test-Mode/kS", 0);
+        this.slot = slot;
+
+        // this.withTimeout(5); TODO: Maybe add?
+    }
+
+    @Override
+    public void initialize() {
+        startPosition = subsystem.getPosition(slot);
+        kG = kS;
+    }
+
+    @Override
+    public void execute() {
+        subsystem.setVolts(kG, slot);
+        kG += 0.001;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.setVolts(0.0, slot);
+        SmartDashboard.putNumber("Test-Mode/kG", kG - kS);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return subsystem.getPosition(slot) > Math.abs(startPosition - 0.1);
+    }
+}

wpi_interface/src/main/java/feedforward_tuning/TuneS.java

@@ -0,0 +1,47 @@
+// TODO: WIP - Not tested
+
+package coppercore.wpi_interface.feedforward_tuning;
+
+import coppercore.controls.Tunable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class TuneS extends Command {
+    private Tunable subsystem;
+
+    private int slot;
+
+    double startPosition;
+
+    double appliedVolts;
+
+    public TuneS(Tunable subsystem, int slot) {
+        this.subsystem = subsystem;
+        this.slot = slot;
+
+        // this.withTimeout(5); TODO: Maybe add?
+    }
+
+    @Override
+    public void initialize() {
+        startPosition = subsystem.getPosition(slot);
+        appliedVolts = 0;
+    }
+
+    @Override
+    public void execute() {
+        subsystem.setVolts(appliedVolts, slot);
+        appliedVolts += 0.001;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.setVolts(0.0, slot);
+        SmartDashboard.putNumber("Test-Mode/kS", appliedVolts);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return subsystem.getVelocity(slot) > 0.01;
+    }
+}

wpi_interface/src/main/java/feedforward_tuning/TuneV.java

@@ -0,0 +1,73 @@
+// TODO: WIP - Not tested
+
+package coppercore.wpi_interface.feedforward_tuning;
+
+import coppercore.controls.Tunable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import java.util.ArrayList;
+
+public class TuneV extends Command {
+    private Tunable subsystem;
+
+    private double volts;
+
+    private int slot;
+    private double conversionFactor;
+
+    private ArrayList<Double> velocities;
+
+    double startPosition; // TODO
+
+    double kS;
+    double pastkV;
+    double average = 0;
+    double vel = 0;
+
+    public TuneV(Tunable subsystem, double volts, int slot) {
+        this.subsystem = subsystem;
+        this.volts = volts;
+        this.slot = slot;
+        this.kS = SmartDashboard.getNumber("Test-Mode/kS", 0);
+        this.pastkV = SmartDashboard.getNumber("Test-Mode/kV", 0);
+
+        this.conversionFactor = subsystem.getConversionFactor(slot);
+
+        this.withTimeout(5);
+    }
+
+    @Override
+    public void initialize() {
+        SmartDashboard.putBoolean("Test-Mode/Ended", false);
+        subsystem.setVolts(volts, slot);
+        velocities = new ArrayList<Double>();
+    }
+
+    @Override
+    public void execute() {
+        vel = subsystem.getVelocity(slot);
+        SmartDashboard.putNumber("Test-Mode/Velocity", vel);
+        if (Math.abs(subsystem.getPosition(slot)) < 0.6 * conversionFactor) {
+            velocities.add(vel);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        SmartDashboard.putBoolean("Test-Mode/Ended", true);
+        subsystem.setVolts(0.0, slot);
+
+        for (double v : velocities) {
+            average += v;
+        }
+
+        average /= velocities.size();
+
+        SmartDashboard.putNumber("Test-Mode/kV", ((volts - kS) / average) + pastkV);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(subsystem.getPosition(slot)) > 0.9 * conversionFactor;
+    }
+}