Skip to content

Commit

Permalink
Convert all aimer radians names to rotations
Browse files Browse the repository at this point in the history
  • Loading branch information
@aidnem
aidnem committed Oct 20, 2024
1 parent 9f54fdc commit d6d8f2d
Show file tree
Hide file tree
Showing 5 changed files with 93 additions and 89 deletions.
27 changes: 11 additions & 16 deletions src/main/java/frc/robot/constants/ScoringConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<Double, Double> getAimerMap() {
HashMap<Double, Double> map = new HashMap<Double, Double>();
map.put(0.0, -0.084);
Expand Down Expand Up @@ -115,10 +110,10 @@ public static HashMap<Double, Double> 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<Double, Double> aimerToleranceTable() {
HashMap<Double, Double> map = new HashMap<Double, Double>();
map.put(0.0, 0.003);
map.put(0.0, 0.003); // Todo: tune this value after conversion from radians to rotations

return map;
}
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/subsystems/scoring/AimerIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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) {}

Expand Down
36 changes: 18 additions & 18 deletions src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand All @@ -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;

Expand Down
45 changes: 26 additions & 19 deletions src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -47,41 +48,44 @@ 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;

double initialAngle = 0.0;
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
Expand All @@ -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();
}
Expand All @@ -120,18 +124,21 @@ 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) {
appliedVolts = overrideVolts;
} 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);
}

Expand Down
Loading

0 comments on commit d6d8f2d

Please sign in to comment.