[Robot] Tune

SouthwestRoboticsProgramming · Apr 1, 2024 · a7e5eb8 · a7e5eb8
1 parent aa41ca0
commit a7e5eb8
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 4 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java
@@ -81,7 +81,8 @@ public final class NTData {
  public static final NTEntry<Double> SHOOTER_PIVOT_KV = new NTDouble("Shooter/Pivot/PID/kV", 0).setPersistent();
  public static final NTEntry<Boolean> SHOOTER_PIVOT_RECALIBRATE = new NTBoolean("Shooter/Pivot/Calibration/Recalibrate", false);
  public static final NTEntry<Double> SHOOTER_PIVOT_CALIBRATE_VOLTS = new NTDouble("Shooter/Pivot/Calibration/Applied Volts", 1).setPersistent();
- public static final NTEntry<Double> SHOOTER_PIVOT_ALLOWABLE_PCT_ERR = new NTDouble("Shooter/Pivot/Allowable Pct Error", 10).setPersistent(); // FIXME TUNE
+// public static final NTEntry<Double> SHOOTER_PIVOT_ALLOWABLE_PCT_ERR = new NTDouble("Shooter/Pivot/Allowable Pct Error", 10).setPersistent(); // FIXME TUNE
+ public static final NTEntry<Double> SHOOTER_PIVOT_ALLOWABLE_ERR = new NTDouble("Shooter/Pivot/Allowable Error (deg)", 1).setPersistent();
  public static final NTEntry<Double> SHOOTER_PIVOT_ANGLE_ADJUST_BLUE = new NTDouble("Shooter/Pivot/Angle Adjust Blue (deg)", -2).setPersistent();
  public static final NTEntry<Double> SHOOTER_PIVOT_ANGLE_ADJUST_RED = new NTDouble("Shooter/Pivot/Angle Adjust Red (deg)", -2).setPersistent();
  public static final NTEntry<Double> SHOOTER_PIVOT_REVERSE_ANGLE = new NTDouble("Shooter/Pivot/Reverse Angle (deg)", 52).setPersistent();

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java
@@ -143,6 +143,8 @@ public void setNeutral() {
 
  @Override
  public void periodic() {
+ position.refresh();
+
  if (NTData.SHOOTER_PIVOT_RECALIBRATE.get()) {
  NTData.SHOOTER_PIVOT_RECALIBRATE.set(false);
  state = State.CALIBRATING;
@@ -180,13 +182,13 @@ public void overrideCalibration(double angleDeg) {
 
  public boolean isAtSetpoint() {
  return state == State.SHOOTING
- && MathUtil.percentError(position.getValue(), setpoint) < NTData.SHOOTER_PIVOT_ALLOWABLE_PCT_ERR.get();
+ && Math.abs(position.getValue() - setpoint) < NTData.SHOOTER_PIVOT_ALLOWABLE_ERR.get() / 360.0;
+// && MathUtil.percentError(position.getValue(), setpoint) < NTData.SHOOTER_PIVOT_ALLOWABLE_PCT_ERR.get();
  }
 
  @Override
  public void simulationPeriodic() {
  motor.updateSim(12);
- position.refresh();
  }
 
  private void applyPID(Slot0Configs config) {

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveConstants.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveConstants.java
@@ -43,7 +43,7 @@ public class SwerveConstants {
 
  private static final double driveGearRatio = (50.0/16) * (16.0/28) * (45.0/15);
  private static final double steerGearRatio = 150.0 / 7;
- private static final double wheelRadiusInches = 1.9 * 0.981162 * 1.022136 * 0.991; // Estimated at first, then fudge-factored to make odom match record
+ private static final double wheelRadiusInches = 1.9 * 0.981162 * 1.022136 * 0.991 * 1.004198; // Estimated at first, then fudge-factored to make odom match record
 
  private static final boolean steerMotorReversed = true;