Add TuneG and TuneS to aimer tuning mode. NEED TO UPDATE TO USE COPPE…

…RCOE

src/main/java/frc/robot/RobotContainer.java

@@ -34,6 +34,8 @@
 import frc.robot.subsystems.scoring.AimerIOSim;
 import frc.robot.subsystems.scoring.ScoringSubsystem;
 import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringAction;
+import frc.robot.utils.feedforward.TuneG;
+import frc.robot.utils.feedforward.TuneS;
 import frc.robot.subsystems.scoring.ShooterIO;
 import frc.robot.subsystems.scoring.ShooterIOSim;
 import frc.robot.subsystems.scoring.ShooterIOTalonFX;
@@ -353,11 +355,11 @@ public void testInit() {
                 scoringSubsystem.setAction(ScoringAction.OVERRIDE);
 
                 // TODO: Add Tunables to coppercore!
-                // masher.a()
-                //         .onTrue(new TuneS(scoringSubsystem, 0));
+                masher.a()
+                        .onTrue(new TuneS(scoringSubsystem, 0));
 
-                // masher.b()
-                //         .onTrue(new TuneG(scoringSubsystem, 0));
+                masher.b()
+                        .onTrue(new TuneG(scoringSubsystem, 0));
 
                 masher.y()
                         .onTrue(

src/main/java/frc/robot/utils/feedforward/TuneG.java

@@ -0,0 +1,50 @@
+// TODO: WIP - Not tested
+
+package frc.robot.utils.feedforward;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import coppercore.controls.Tunable;
+
+//TODO: USE COPPERCORE VERSION
+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);
+    }
+}

src/main/java/frc/robot/utils/feedforward/TuneS.java

@@ -0,0 +1,48 @@
+// TODO: WIP - Not tested
+
+package frc.robot.utils.feedforward;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import coppercore.controls.Tunable;
+
+//TODO: USE COPPERCORE VERSION
+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;
+    }
+}

src/main/java/frc/robot/utils/feedforward/TuneV.java

@@ -0,0 +1,74 @@
+// TODO: WIP - Not tested
+
+package frc.robot.utils.feedforward;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import coppercore.controls.Tunable;
+import java.util.ArrayList;
+
+//TODO: USE COPPERCORE VERSION
+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;
+    }
+}