From 136140185cfbf1ec113eb91013adf300964688c9 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Wed, 9 Oct 2024 16:38:51 +0000 Subject: [PATCH 1/2] Add TuneG, TuneS, and TuneV to wpi_interface.feedforward_tuning Also add coppercore.controls as a dependency of wpi_interface. UNTESTED - This code needs to be built and tested before it is used, as it wouldn't build properly in GitHub codespaces. --- wpi_interface/build.gradle | 3 + .../main/java/feedforward_tuning/TuneG.java | 47 ++++++++++++ .../main/java/feedforward_tuning/TuneS.java | 47 ++++++++++++ .../main/java/feedforward_tuning/TuneV.java | 73 +++++++++++++++++++ 4 files changed, 170 insertions(+) create mode 100644 wpi_interface/src/main/java/feedforward_tuning/TuneG.java create mode 100644 wpi_interface/src/main/java/feedforward_tuning/TuneS.java create mode 100644 wpi_interface/src/main/java/feedforward_tuning/TuneV.java diff --git a/wpi_interface/build.gradle b/wpi_interface/build.gradle index 1d47afe..b56676a 100644 --- a/wpi_interface/build.gradle +++ b/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,6 +19,8 @@ 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' diff --git a/wpi_interface/src/main/java/feedforward_tuning/TuneG.java b/wpi_interface/src/main/java/feedforward_tuning/TuneG.java new file mode 100644 index 0000000..8f5c20f --- /dev/null +++ b/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); + } +} diff --git a/wpi_interface/src/main/java/feedforward_tuning/TuneS.java b/wpi_interface/src/main/java/feedforward_tuning/TuneS.java new file mode 100644 index 0000000..80702d6 --- /dev/null +++ b/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; + } +} diff --git a/wpi_interface/src/main/java/feedforward_tuning/TuneV.java b/wpi_interface/src/main/java/feedforward_tuning/TuneV.java new file mode 100644 index 0000000..f5cabbb --- /dev/null +++ b/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 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(); + } + + @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; + } +} From 8b3b9c0243b7ea38241932eeddb2dc6b21f5ba27 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 12 Nov 2024 14:41:26 +0000 Subject: [PATCH 2/2] Add import for wpiutil to build.gradle so that sendable is included --- wpi_interface/build.gradle | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/wpi_interface/build.gradle b/wpi_interface/build.gradle index b56676a..077a479 100644 --- a/wpi_interface/build.gradle +++ b/wpi_interface/build.gradle @@ -24,6 +24,8 @@ 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' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' @@ -40,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()