From d6d8f2de1d616d2cc332289024c99b3917902d72 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sun, 20 Oct 2024 16:27:04 -0400 Subject: [PATCH] Convert all aimer radians names to rotations --- .../frc/robot/constants/ScoringConstants.java | 27 ++++----- .../frc/robot/subsystems/scoring/AimerIO.java | 16 ++--- .../subsystems/scoring/AimerIORoboRio.java | 36 ++++++------ .../robot/subsystems/scoring/AimerIOSim.java | 45 ++++++++------ .../subsystems/scoring/ScoringSubsystem.java | 58 ++++++++++--------- 5 files changed, 93 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index 84f75e6..423f30a 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -41,27 +41,22 @@ public class ScoringConstants { public static final double kickerIntakeVolts = 2.0; - public static final double aimPositionTolerance = 0.003; + // public static final double aimPositionTolerance = 0.003; - // These values have been reduced for tuning because we can't set a voltage limit on the motors - // anymore - // public static final double aimerAcceleration = 4.5; // TODO: 15.0 - // public static final double aimerCruiseVelocity = 7.0; // TODO: 15.0 - public static final double aimerAcceleration = 3.5; // 0.7; - public static final double aimerCruiseVelocity = 0.8; // 0.4; + public static final double aimerAcceleration = 3.5; + public static final double aimerCruiseVelocity = 0.8; public static final double shooterLowerVelocityMarginRPM = 50; public static final double shooterUpperVelocityMarginRPM = 150; - public static final double aimAngleMarginRadians = Units.degreesToRadians(1); - public static final double aimAngleVelocityMargin = 2.0; // Units.degreesToRadians(5); + public static final double aimAngleMarginRotations = Units.degreesToRotations(1); + public static final double aimAngleVelocityMargin = 2.0; - public static final double intakeAngleToleranceRadians = 0.05; - // Math.PI / 2 - Units.degreesToRadians(40); + public static final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value public static final double shooterAmpVelocityRPM = 2000; - public static final double aimMaxAngleRadians = 0.361328 - 0.184570; // Math.PI / 2 - public static final double aimMinAngleRadians = -0.195; // -0.037598 - 0.184570; + public static final double aimMaxAngleRotations = 0.361328 - 0.184570; + public static final double aimMinAngleRotations = -0.195; public static final double aimAngleTolerance = 0.015; public static final double maxAimIntake = 0.0; @@ -74,7 +69,7 @@ public class ScoringConstants { // NOTE - This should be monotonically increasing // Key - Distance in meters - // Value - Aimer angle in radians + // Value - Aimer angle in rotations public static HashMap getAimerMap() { HashMap map = new HashMap(); map.put(0.0, -0.084); @@ -115,10 +110,10 @@ public static HashMap timeToGoalMap() { // NOTE - This should be monotonically increasing // Key - Distance to goal in meters - // Value - Aimer angle tolerance in radians + // Value - Aimer angle tolerance in rotations public static HashMap aimerToleranceTable() { HashMap map = new HashMap(); - map.put(0.0, 0.003); + map.put(0.0, 0.003); // Todo: tune this value after conversion from radians to rotations return map; } diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java index 07baea1..285f61b 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java @@ -5,15 +5,15 @@ public interface AimerIO { @AutoLog public static class AimerInputs { - public double aimAngleRad = 0.0; - public double aimGoalAngleRad = 0.0; - public double aimProfileGoalAngleRad = 0.0; + public double aimAngleRot = 0.0; + public double aimGoalAngleRot = 0.0; + public double aimProfileGoalAngleRot = 0.0; public double aimStatorCurrentAmps = 0.0; public double aimSupplyCurrentAmps = 0.0; - public double aimVelocityRadPerSec = 0.0; - public double aimVelocityErrorRadPerSec = 0.0; + public double aimVelocityRotPerSec = 0.0; + public double aimVelocityErrorRotPerSec = 0.0; } @AutoLog @@ -25,11 +25,11 @@ public default void updateInputs(AimerInputs inputs) {} public default void applyOutputs(AimerOutputs outputs) {} - public default void setAimAngleRad(double angle) {} + public default void setAimAngleRot(double angle) {} - public default void controlAimAngleRad() {} + public default void controlAimAngleRot() {} - public default void setAngleClampsRad(double min, double max) {} + public default void setAngleClampsRot(double min, double max) {} public default void setOverrideMode(boolean override) {} diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 32aac8b..182cf07 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -42,7 +42,7 @@ public class AimerIORoboRio implements AimerIO { double minAngleClamp = 0.0; double maxAngleClamp = 0.0; - double goalAngleRad = 0.0; + double goalAngleRot = 0.0; double appliedVolts = 0.0; double velocity = 0.0; @@ -99,36 +99,36 @@ public AimerIORoboRio() { public void resetPID() {} @Override - public void setAimAngleRad(double goalAngleRad) { - this.goalAngleRad = goalAngleRad; + public void setAimAngleRot(double goalAngleRot) { + this.goalAngleRot = goalAngleRot; } @Override - public void controlAimAngleRad() { - if (goalAngleRad != previousGoalAngle) { + public void controlAimAngleRot() { + if (goalAngleRot != previousGoalAngle) { timer.reset(); timer.start(); - previousGoalAngle = goalAngleRad; + previousGoalAngle = goalAngleRot; } - goalAngleRad = MathUtil.clamp(goalAngleRad, minAngleClamp, maxAngleClamp); + goalAngleRot = MathUtil.clamp(goalAngleRot, minAngleClamp, maxAngleClamp); } @Override - public void setAngleClampsRad(double minAngleClamp, double maxAngleClamp) { + public void setAngleClampsRot(double minAngleClamp, double maxAngleClamp) { if (minAngleClamp > maxAngleClamp) { return; } this.minAngleClamp = MathUtil.clamp( minAngleClamp, - ScoringConstants.aimMinAngleRadians, - ScoringConstants.aimMaxAngleRadians); + ScoringConstants.aimMinAngleRotations, + ScoringConstants.aimMaxAngleRotations); this.maxAngleClamp = MathUtil.clamp( maxAngleClamp, - ScoringConstants.aimMinAngleRadians, - ScoringConstants.aimMaxAngleRadians); + ScoringConstants.aimMinAngleRotations, + ScoringConstants.aimMaxAngleRotations); } @Override @@ -207,19 +207,19 @@ public void updateInputs(AimerInputs inputs) { Logger.recordOutput("Scoring/motorDisabled", motorDisabled); - inputs.aimGoalAngleRad = goalAngleRad; - inputs.aimProfileGoalAngleRad = controlSetpoint; - inputs.aimAngleRad = getEncoderPosition(); + inputs.aimGoalAngleRot = goalAngleRot; + inputs.aimProfileGoalAngleRot = controlSetpoint; + inputs.aimAngleRot = getEncoderPosition(); double currentTime = Utils.getCurrentTimeSeconds(); double diffTime = currentTime - lastTime; lastTime = currentTime; - inputs.aimVelocityRadPerSec = (getEncoderPosition() - lastPosition) / diffTime; + inputs.aimVelocityRotPerSec = (getEncoderPosition() - lastPosition) / diffTime; velocity = (getEncoderPosition() - lastPosition) / diffTime; lastPosition = getEncoderPosition(); - inputs.aimVelocityErrorRadPerSec = + inputs.aimVelocityErrorRotPerSec = ((getEncoderPosition() - controlSetpoint) - lastError) / diffTime; lastError = getEncoderPosition() - controlSetpoint; @@ -230,7 +230,7 @@ public void updateInputs(AimerInputs inputs) { @Override public void applyOutputs(AimerOutputs outputs) { MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0); - motionMagicVoltage.withPosition(goalAngleRad); + motionMagicVoltage.withPosition(goalAngleRot); request = motionMagicVoltage; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java index 9173f28..b4f31c1 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.constants.ScoringConstants; @@ -47,7 +48,7 @@ public class AimerIOSim implements AimerIO { double minAngleClamp = 0.0; double maxAngleClamp = 0.0; - double goalAngleRad = 0.0; + double goalAngleRot = 0.0; double controlSetpoint = 0.0; double appliedVolts = 0.0; @@ -55,33 +56,36 @@ public class AimerIOSim implements AimerIO { double initialVelocity = 0.0; @Override - public void setAimAngleRad(double goalAngleRad) { - this.goalAngleRad = goalAngleRad; + public void setAimAngleRot(double goalAngleRot) { + this.goalAngleRot = goalAngleRot; } @Override - public void controlAimAngleRad() { - if (goalAngleRad != previousGoalAngle) { + public void controlAimAngleRot() { + if (goalAngleRot != previousGoalAngle) { timer.reset(); timer.start(); - initialAngle = sim.getAngleRads(); - initialVelocity = sim.getVelocityRadPerSec(); + initialAngle = Units.radiansToRotations(sim.getAngleRads()); + initialVelocity = Units.radiansToRotations(sim.getVelocityRadPerSec()); - previousGoalAngle = goalAngleRad; + previousGoalAngle = goalAngleRot; } - goalAngleRad = MathUtil.clamp(goalAngleRad, minAngleClamp, maxAngleClamp); + goalAngleRot = MathUtil.clamp(goalAngleRot, minAngleClamp, maxAngleClamp); } @Override - public void setAngleClampsRad(double minAngleClamp, double maxAngleClamp) { + public void setAngleClampsRot(double minAngleClamp, double maxAngleClamp) { + // TODO: Figure out why we needed this method, and figure out how to make it work if (minAngleClamp > maxAngleClamp) { return; } + // These two lines look like they should make the minimum angle 0.0 rotations, + // which would mean that the arm could no longer go below horizontal. this.minAngleClamp = - MathUtil.clamp(minAngleClamp, 0.0, ScoringConstants.aimMaxAngleRadians); + MathUtil.clamp(minAngleClamp, 0.0, ScoringConstants.aimMaxAngleRotations); this.maxAngleClamp = - MathUtil.clamp(maxAngleClamp, 0.0, ScoringConstants.aimMaxAngleRadians); + MathUtil.clamp(maxAngleClamp, 0.0, ScoringConstants.aimMaxAngleRotations); } @Override @@ -105,11 +109,11 @@ public void setPID(double p, double i, double d) { public void updateInputs(AimerInputs inputs) { sim.update(SimConstants.loopTime); - inputs.aimGoalAngleRad = goalAngleRad; - inputs.aimProfileGoalAngleRad = controlSetpoint; - inputs.aimAngleRad = sim.getAngleRads(); + inputs.aimGoalAngleRot = goalAngleRot; + inputs.aimProfileGoalAngleRot = controlSetpoint; + inputs.aimAngleRot = Units.radiansToRotations(sim.getAngleRads()); - inputs.aimVelocityRadPerSec = sim.getVelocityRadPerSec(); + inputs.aimVelocityRotPerSec = Units.radiansToRotations(sim.getVelocityRadPerSec()); inputs.aimStatorCurrentAmps = sim.getCurrentDrawAmps(); } @@ -120,10 +124,12 @@ public void applyOutputs(AimerOutputs outputs) { profile.calculate( timer.get(), new State(initialAngle, initialVelocity), - new State(goalAngleRad, 0)); + new State(goalAngleRot, 0)); double controlSetpoint = MathUtil.clamp( - trapezoidSetpoint.position, 0.0, ScoringConstants.aimMaxAngleRadians); + trapezoidSetpoint.position, + ScoringConstants.aimMinAngleRotations, + ScoringConstants.aimMaxAngleRotations); double velocitySetpoint = trapezoidSetpoint.velocity; if (override) { @@ -131,7 +137,8 @@ public void applyOutputs(AimerOutputs outputs) { } else { appliedVolts = feedforward.calculate(controlSetpoint, velocitySetpoint) - + controller.calculate(sim.getAngleRads(), controlSetpoint); + + controller.calculate( + Units.radiansToRotations(sim.getAngleRads()), controlSetpoint); appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0); } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 02d4510..13be5cd 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -112,8 +112,8 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo) { aimerInterpolated = new InterpolateDouble( ScoringConstants.getAimerMap(), - ScoringConstants.aimMinAngleRadians, - ScoringConstants.aimMaxAngleRadians); + ScoringConstants.aimMinAngleRotations, + ScoringConstants.aimMaxAngleRotations); aimerAngleTolerance = new InterpolateDouble(ScoringConstants.aimerToleranceTable()); @@ -133,11 +133,13 @@ public void setBrakeMode(boolean brake) { } public boolean atAimerGoalPosition() { - return Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad) < 0.2; + return Math.abs(aimerInputs.aimAngleRot - aimerInputs.aimGoalAngleRot) < 0.2; } private void idle() { - aimerIo.setAimAngleRad(ScoringConstants.aimMinAngleRadians + 0.001); + // aimerIo.setAimAngleRad(ScoringConstants.aimMinAngleRotations + 0.001); + // Todo: if we can trust aimer, figure out if we really need to add 0.001 to the min angle + aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations); shooterIo.setShooterVelocityRPM(0); shooterIo.setKickerVolts(0); @@ -148,7 +150,7 @@ private void idle() { Logger.recordOutput("scoring/aimGoal", 0.0); SmartDashboard.putBoolean("Has Note", shooterInputs.noteInShooter); - SmartDashboard.putNumber("Aimer Location", aimerInputs.aimAngleRad); + SmartDashboard.putNumber("Aimer Location", aimerInputs.aimAngleRot); if ((!hasNote() || overrideIntake) && action == ScoringAction.INTAKE) { state = ScoringState.INTAKE; @@ -176,7 +178,7 @@ private void idle() { private void intake() { if (!aimerAtIntakePosition()) { - aimerIo.setAimAngleRad(ScoringConstants.aimMinAngleRadians); + aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations); } if (!hasNote()) { @@ -191,7 +193,7 @@ private void intake() { } private void sourceIntake() { - aimerIo.setAimAngleRad(0.35); + aimerIo.setAimAngleRot(0.35); shooterIo.setKickerVolts(-1); shooterIo.setOverrideMode(true); @@ -226,7 +228,7 @@ private void prime() { double distanceToGoal = findDistanceToGoal(); Logger.recordOutput("scoring/aimGoal", getAimerAngle(distanceToGoal)); shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distanceToGoal)); - aimerIo.setAimAngleRad(getAimerAngle(distanceToGoal)); + aimerIo.setAimAngleRot(getAimerAngle(distanceToGoal)); if (!overrideBeamBreak) { shooterIo.setKickerVolts(hasNote() ? 0.0 : ScoringConstants.kickerIntakeVolts); } @@ -239,9 +241,9 @@ private void prime() { > (shooterOutputs.shooterLeftGoalVelocityRPM - ScoringConstants.shooterLowerVelocityMarginRPM); boolean aimReady = - Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad) + Math.abs(aimerInputs.aimAngleRot - aimerInputs.aimGoalAngleRot) < aimerAngleTolerance.getValue(distanceToGoal) - && Math.abs(aimerInputs.aimVelocityErrorRadPerSec) + && Math.abs(aimerInputs.aimVelocityErrorRotPerSec) < ScoringConstants.aimAngleVelocityMargin; boolean driveReady = driveAlignedSupplier.get(); boolean fieldLocationReady = true; @@ -293,7 +295,7 @@ private void prime() { private void ampPrime() { // shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); // TODO: Test this out - aimerIo.setAimAngleRad( + aimerIo.setAimAngleRot( 0.25); // This is actually in rotations and not radians, I really need to rename all // of the aimer IO functions if (action != ScoringAction.SHOOT && action != ScoringAction.AMP_AIM) { @@ -312,7 +314,7 @@ private void shoot() { double shootRPM = shooterInterpolated.getValue(distancetoGoal); shooterIo.setShooterVelocityRPM(shootRPM); double aimAngleRad = aimerInterpolated.getValue(distancetoGoal); - aimerIo.setAimAngleRad(aimAngleRad); + aimerIo.setAimAngleRot(aimAngleRad); shooterIo.setKickerVolts(12); @@ -338,7 +340,7 @@ private void ampShoot() { } private void endgame() { - aimerIo.setAimAngleRad(Math.PI / 2); + aimerIo.setAimAngleRot(Math.PI / 2); shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); shooterIo.setKickerVolts(action == ScoringAction.TRAP_SCORE ? 10 : 0); if (action != ScoringAction.ENDGAME && action != ScoringAction.TRAP_SCORE) { @@ -350,7 +352,7 @@ private void tuning() { shooterGoalVelocityRPMTuning = SmartDashboard.getNumber("Test-Mode/ShooterGoal", 0.0); aimerGoalAngleRadTuning = SmartDashboard.getNumber("Test-Mode/AimerGoal", 0.0); shooterIo.setShooterVelocityRPM(shooterGoalVelocityRPMTuning); - aimerIo.setAimAngleRad(aimerGoalAngleRadTuning); + aimerIo.setAimAngleRot(aimerGoalAngleRadTuning); shooterIo.setKickerVolts(kickerVoltsTuning); if (action != ScoringAction.TUNING) { @@ -401,9 +403,9 @@ public boolean hasNote() { } public boolean aimerAtIntakePosition() { - return aimerInputs.aimAngleRad - < ScoringConstants.aimMinAngleRadians - + ScoringConstants.intakeAngleToleranceRadians; + return aimerInputs.aimAngleRot + < ScoringConstants.aimMinAngleRotations + + ScoringConstants.intakeAngleToleranceRotations; // return true;\][] } @@ -432,7 +434,7 @@ public void enabledInit() { setOverrideStageAvoidance(false); setOverrideShoot(false); - aimerIo.setAimAngleRad(aimerInputs.aimAngleRad); + aimerIo.setAimAngleRot(aimerInputs.aimAngleRot); } @Override @@ -449,17 +451,17 @@ public void periodic() { overrideBeamBreak = SmartDashboard.getBoolean("Beam Break Overridden", overrideBeamBreak); if (state == ScoringState.TEMPORARY_SETPOINT) { - aimerIo.setAngleClampsRad( - ScoringConstants.aimMinAngleRadians, ScoringConstants.aimMaxAngleRadians); + aimerIo.setAngleClampsRot( + ScoringConstants.aimMinAngleRotations, ScoringConstants.aimMaxAngleRotations); } else if (state != ScoringState.TUNING && state != ScoringState.ENDGAME && state != ScoringState.IDLE // && Math.abs(elevatorPositionSupplier.getAsDouble()) < 0.2 && !overrideStageAvoidance) { - aimerIo.setAngleClampsRad(ScoringConstants.aimMinAngleRadians, 0); + aimerIo.setAngleClampsRot(ScoringConstants.aimMinAngleRotations, 0); } - aimerIo.controlAimAngleRad(); + aimerIo.controlAimAngleRot(); aimerIo.setOverrideMode(state == ScoringState.OVERRIDE); shooterIo.setOverrideMode(state == ScoringState.OVERRIDE); @@ -469,7 +471,7 @@ public void periodic() { Logger.recordOutput( "scoring/Aimer3d", - new Pose3d(-0.255, 0.2, 0.502, new Rotation3d(0, -aimerInputs.aimAngleRad, 0))); + new Pose3d(-0.255, 0.2, 0.502, new Rotation3d(0, -aimerInputs.aimAngleRot, 0))); Logger.recordOutput("scoring/readyToShoot", readyToShoot); Logger.recordOutput("scoring/overrideShoot", overrideShoot); @@ -478,7 +480,7 @@ public void periodic() { Logger.recordOutput("scoring/distance", findDistanceToGoal()); if (ModeConstants.currentMode == Mode.SIM) { - aimMechanism.setAngle(Units.radiansToDegrees(aimerInputs.aimAngleRad)); + aimMechanism.setAngle(Units.radiansToDegrees(aimerInputs.aimAngleRot)); Logger.recordOutput("scoring/mechanism2d", mechanism); } @@ -524,7 +526,7 @@ public void periodic() { // If the robot is disabled, the pid should not be winding up if (DriverStation.isDisabled()) { aimerIo.resetPID(); - aimerIo.setAimAngleRad(aimerInputs.aimAngleRad); + aimerIo.setAimAngleRot(aimerInputs.aimAngleRot); } shooterIo.updateInputs(shooterInputs); @@ -573,7 +575,7 @@ public double getPosition(int slot) { switch (slot) { // Aimer case 0: - return aimerInputs.aimAngleRad; + return aimerInputs.aimAngleRot; // Shooter case 1: return shooterInputs.shooterLeftVelocityRPM; @@ -587,7 +589,7 @@ public double getVelocity(int slot) { switch (slot) { // Aimer case 0: - return aimerInputs.aimVelocityRadPerSec; + return aimerInputs.aimVelocityRotPerSec; // Shooter case 1: return shooterInputs.shooterLeftVelocityRPM; @@ -680,7 +682,7 @@ public void runToPosition(double position, int slot) { switch (slot) { // Aimer case 0: - aimerIo.setAimAngleRad(temporarySetpointPosition); + aimerIo.setAimAngleRot(temporarySetpointPosition); break; // Shooter case 1: