diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index 8081bd68..e50d09c6 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -99,7 +99,7 @@ public void print() { DriverStation.reportWarning("FF Characterization Results:", false); DriverStation.reportWarning("\tCount=" + Integer.toString(velocityData.size()) + "", false); - DriverStation.reportWarning(String.format("\tR2=%.5f", regression.R2()), false); + DriverStation.reportWarning(String.format("\tR2=%.5f", regression.r2()), false); DriverStation.reportWarning(String.format("\tkS=%.5f", regression.beta(0)), false); DriverStation.reportWarning(String.format("\tkV=%.5f", regression.beta(1)), false); } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java index 07c482c2..8a9ed623 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java @@ -50,7 +50,7 @@ public class ModuleIOSparkMax implements ModuleIO { private final Queue drivePositionQueue; private final Queue turnPositionQueue; - private final boolean isTurnMotorInverted = true; + private static final boolean TURN_MOTOR_INVERTED = true; private final Rotation2d absoluteEncoderOffset; public ModuleIOSparkMax(int index) { @@ -80,7 +80,7 @@ public ModuleIOSparkMax(int index) { absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; default: - throw new RuntimeException("Invalid module index"); + throw new UnsupportedOperationException("Invalid module index"); } driveSparkMax.restoreFactoryDefaults(); @@ -92,7 +92,7 @@ public ModuleIOSparkMax(int index) { driveEncoder = driveSparkMax.getEncoder(); turnRelativeEncoder = turnSparkMax.getEncoder(); - turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkMax.setInverted(TURN_MOTOR_INVERTED); driveSparkMax.setSmartCurrentLimit(40); turnSparkMax.setSmartCurrentLimit(30); driveSparkMax.enableVoltageCompensation(12.0); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index ed183e4e..3fb04620 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -43,6 +43,7 @@ * absolute encoders using AdvantageScope. These values are logged under * "/Drive/ModuleX/TurnAbsolutePositionRad" */ + public class ModuleIOTalonFX implements ModuleIO { private final TalonFX m_driveTalon; private final TalonFX m_turnTalon; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 366c94a5..38d32e0c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -3,14 +3,12 @@ import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { - - @AutoLog - class ShooterIOInputs { - } - default void setMotorVoltageTL(double voltage) {} - default void setMotorVoltageTR(double voltage) {} - default void setMotorVoltageBL(double voltage) {} - default void setMotorVoltageBR(double voltage) {} - default void setKickerVoltage(double voltage) {} + @AutoLog + class ShooterIOInputs {} + default void setMotorVoltageTL(double voltage) {} + default void setMotorVoltageTR(double voltage) {} + default void setMotorVoltageBL(double voltage) {} + default void setMotorVoltageBR(double voltage) {} + default void setKickerVoltage(double voltage) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index b796facd..1603cdb9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -5,47 +5,47 @@ import com.revrobotics.CANSparkMax; public class ShooterIOPrototype implements ShooterIO { - private final CANSparkFlex m_topLeftMotor; - private final CANSparkFlex m_topRightMotor; - private final CANSparkMax m_bottomLeftMotor; - private final CANSparkMax m_bottomRightMotor; - private final CANSparkMax m_kickekMotor; - public ShooterIOPrototype() { - m_topLeftMotor = new CANSparkFlex(13, CANSparkLowLevel.MotorType.kBrushless); - m_topRightMotor = new CANSparkFlex(14, CANSparkLowLevel.MotorType.kBrushless); - m_bottomLeftMotor = new CANSparkMax(15, CANSparkLowLevel.MotorType.kBrushless); - m_bottomRightMotor = new CANSparkMax(16, CANSparkLowLevel.MotorType.kBrushless); - m_kickekMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); - - m_topLeftMotor.setInverted(false); - m_bottomLeftMotor.setInverted(true); - - m_topLeftMotor.burnFlash(); - m_topRightMotor.burnFlash(); - } - - @Override - public void setMotorVoltageTL(double voltage) { - m_topLeftMotor.setVoltage(voltage); - } - - @Override - public void setMotorVoltageTR(double voltage) { - m_topRightMotor.setVoltage(voltage); - } - - @Override - public void setMotorVoltageBL(double voltage) { - m_bottomLeftMotor.setVoltage(voltage); - } - - @Override - public void setMotorVoltageBR(double voltage) { - m_bottomRightMotor.setVoltage(voltage); - } - - @Override - public void setKickerVoltage(double voltage) { - m_kickekMotor.setVoltage(voltage); - } + private final CANSparkFlex m_topLeftMotor; + private final CANSparkFlex m_topRightMotor; + private final CANSparkMax m_bottomLeftMotor; + private final CANSparkMax m_bottomRightMotor; + private final CANSparkMax m_kickekMotor; + public ShooterIOPrototype() { + m_topLeftMotor = new CANSparkFlex(13, CANSparkLowLevel.MotorType.kBrushless); + m_topRightMotor = new CANSparkFlex(14, CANSparkLowLevel.MotorType.kBrushless); + m_bottomLeftMotor = new CANSparkMax(15, CANSparkLowLevel.MotorType.kBrushless); + m_bottomRightMotor = new CANSparkMax(16, CANSparkLowLevel.MotorType.kBrushless); + m_kickekMotor = new CANSparkMax(17, CANSparkLowLevel.MotorType.kBrushless); + + m_topLeftMotor.setInverted(false); + m_bottomLeftMotor.setInverted(true); + + m_topLeftMotor.burnFlash(); + m_topRightMotor.burnFlash(); + } + + @Override + public void setMotorVoltageTL(double voltage) { + m_topLeftMotor.setVoltage(voltage); + } + + @Override + public void setMotorVoltageTR(double voltage) { + m_topRightMotor.setVoltage(voltage); + } + + @Override + public void setMotorVoltageBL(double voltage) { + m_bottomLeftMotor.setVoltage(voltage); + } + + @Override + public void setMotorVoltageBR(double voltage) { + m_bottomRightMotor.setVoltage(voltage); + } + + @Override + public void setKickerVoltage(double voltage) { + m_kickekMotor.setVoltage(voltage); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index d32b3733..ad39de26 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -6,34 +6,36 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - private final ShooterIO m_io; - public ShooterSubsystem(ShooterIO io) { - m_io = io; - SmartDashboard.putNumber("Top wheel ratio", 0.8); - SmartDashboard.putNumber("Left wheel", 0.6); - SmartDashboard.putNumber("Right wheel", 0.6); - } - - public void setShooterPowerLeft(double power) { - m_io.setMotorVoltageTL((power * 12.0) * SmartDashboard.getNumber("Top wheel ratio", 0.85)); - m_io.setMotorVoltageBL(power * 12.0); - } - - public void setShooterPowerRight(double power) { - m_io.setMotorVoltageTR((power * 12.0) * SmartDashboard.getNumber("Top wheel ratio", 0.85)); - m_io.setMotorVoltageBR(power * 12.0); - } - - public void setKickerPower(double power) { - m_io.setKickerVoltage(power * 12.0); - } - - public Command setShooterPower(double left, double right) { - return run(() -> { - setShooterPowerLeft(left == 0.0 ? 0.0 : SmartDashboard.getNumber("Left wheel", 0.6)); - setShooterPowerRight(right == 0.0 ? 0.0 : SmartDashboard.getNumber("Right wheel", 0.6)); - setKickerPower(left == 0.0 ? 0.0 : 1.0); - }); - } + private final ShooterIO m_io; + + private static final String TOP_WHEEL_RATIO = "Top wheel ratio"; + public ShooterSubsystem(ShooterIO io) { + m_io = io; + SmartDashboard.putNumber(TOP_WHEEL_RATIO, 0.8); + SmartDashboard.putNumber("Left wheel", 0.6); + SmartDashboard.putNumber("Right wheel", 0.6); + } + + public void setShooterPowerLeft(double power) { + m_io.setMotorVoltageTL((power * 12.0) * SmartDashboard.getNumber(TOP_WHEEL_RATIO, 0.85)); + m_io.setMotorVoltageBL(power * 12.0); + } + + public void setShooterPowerRight(double power) { + m_io.setMotorVoltageTR((power * 12.0) * SmartDashboard.getNumber(TOP_WHEEL_RATIO, 0.85)); + m_io.setMotorVoltageBR(power * 12.0); + } + + public void setKickerPower(double power) { + m_io.setKickerVoltage(power * 12.0); + } + + public Command setShooterPower(double left, double right) { + return run(() -> { + setShooterPowerLeft(left == 0.0 ? 0.0 : SmartDashboard.getNumber("Left wheel", 0.6)); + setShooterPowerRight(right == 0.0 ? 0.0 : SmartDashboard.getNumber("Right wheel", 0.6)); + setKickerPower(left == 0.0 ? 0.0 : 1.0); + }); + } } diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 37222f23..58520e26 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -100,9 +100,9 @@ public void setDynamicObstacles( } private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); + protected LocalADStar adStar = new LocalADStar(); + protected boolean isNewPathAvailable = false; + protected List currentPathPoints = Collections.emptyList(); @Override public void toLog(LogTable table) { diff --git a/src/main/java/frc/robot/util/PolynomialRegression.java b/src/main/java/frc/robot/util/PolynomialRegression.java index 28c57967..2bc44b4c 100644 --- a/src/main/java/frc/robot/util/PolynomialRegression.java +++ b/src/main/java/frc/robot/util/PolynomialRegression.java @@ -2,6 +2,7 @@ import Jama.Matrix; import Jama.QRDecomposition; +import edu.wpi.first.wpilibj.DriverStation; // NOTE: This file is available at // http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html @@ -25,8 +26,8 @@ public class PolynomialRegression implements Comparable { private final String variableName; // name of the predictor variable private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error + private final Matrix beta; // the polynomial regression coefficients + private final double sse; // sum of squares due to error private double sst; // total sum of squares /** @@ -74,7 +75,9 @@ public PolynomialRegression(double[] x, double[] y, int degree, String variableN // find least squares solution qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; + if (qr.isFullRank()) { + break; + } // decrease degree and try again this.degree--; @@ -88,7 +91,9 @@ public PolynomialRegression(double[] x, double[] y, int degree, String variableN // mean of y[] values double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; + for (int i = 0; i < n; i++) { + sum += y[i]; + } double mean = sum / n; // total variation to be accounted for @@ -110,7 +115,9 @@ public PolynomialRegression(double[] x, double[] y, int degree, String variableN */ public double beta(int j) { // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; + if (Math.abs(beta.get(j, 0)) < 1E-4) { + return 0.0; + } return beta.get(j, 0); } @@ -129,8 +136,10 @@ public int degree() { * @return the coefficient of determination R2, which is a real number between * 0 and 1 */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function + public double r2() { + if (sst == 0.0) { + return 1.0; // constant function + } return 1.0 - sse / sst; } @@ -143,7 +152,9 @@ public double R2() { public double predict(double x) { // horner's method double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + for (int j = degree; j >= 0; j--) { + y = beta(j) + (x * y); + } return y; } @@ -158,38 +169,73 @@ public String toString() { int j = degree; // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + while (j >= 0 && Math.abs(beta(j)) < 1E-5) { + j--; + } // create remaining terms while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); + if (j == 0) { + s.append(String.format("%.10f ", beta(j))); + } + else if (j == 1) { + s.append(String.format("%.10f %s + ", beta(j), variableName)); + } else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); j--; } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + s.append(" (R^2 = ").append(String.format("%.3f", r2())).append(")"); - // replace "+ -2n" with "- 2n" + // replace "+ -2n" with "- 2n" return s.toString().replace("+ -", "- "); } /** Compare lexicographically. */ public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; + double lEPSILON = 1E-5; int maxDegree = Math.max(this.degree(), that.degree()); for (int j = maxDegree; j >= 0; j--) { double term1 = 0.0; double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; + if (this.degree() >= j) { + term1 = this.beta(j); + } + if (that.degree() >= j) { + term2 = that.beta(j); + } + if (Math.abs(term1) < lEPSILON) { + term1 = 0.0; + } + if (Math.abs(term2) < lEPSILON) { + term2 = 0.0; + } + if (term1 < term2) { + return -1; + } + else if (term1 > term2) { + return +1; + } } return 0; } + @Override + public boolean equals(Object o) { + if (o == null) { + return false; + } + if (o.getClass() == this.getClass()) { + return this.compareTo((PolynomialRegression) o) == 0; + } else { + return false; + } + } + + @Override + public int hashCode() { + return super.hashCode(); + } + /** * Unit tests the {@code PolynomialRegression} data type. * @@ -200,6 +246,6 @@ public static void main(String[] args) { double[] y = {100, 350, 1500, 6700, 20160, 40000}; PolynomialRegression regression = new PolynomialRegression(x, y, 3); - System.out.println(regression); + DriverStation.reportWarning(String.valueOf(regression), false); } }