diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8501326..df4512e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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( diff --git a/src/main/java/frc/robot/utils/feedforward/TuneG.java b/src/main/java/frc/robot/utils/feedforward/TuneG.java new file mode 100644 index 0000000..5522ed8 --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc/robot/utils/feedforward/TuneS.java b/src/main/java/frc/robot/utils/feedforward/TuneS.java new file mode 100644 index 0000000..cc81ebe --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/utils/feedforward/TuneV.java b/src/main/java/frc/robot/utils/feedforward/TuneV.java new file mode 100644 index 0000000..5817686 --- /dev/null +++ b/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 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; + } +}