diff --git a/CompBot/build.gradle b/CompBot/build.gradle index 0d37364..a316393 100644 --- a/CompBot/build.gradle +++ b/CompBot/build.gradle @@ -61,7 +61,6 @@ configurations { // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { - implementation 'org.apache.commons:commons-math3:3.6.1' annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/CompBot/src/main/java/frc/robot/Constants.java b/CompBot/src/main/java/frc/robot/Constants.java index 6591c63..5716c79 100644 --- a/CompBot/src/main/java/frc/robot/Constants.java +++ b/CompBot/src/main/java/frc/robot/Constants.java @@ -1,16 +1,36 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.units.measure.Distance; +import frc.robot.aiming.AimConstraints; +import frc.robot.aiming.AimStrategy; +import frc.robot.aiming.PhysicsAim; public class Constants { // Checked the FIRST Game Manual and fixed the field dimensions. public static class FieldConstants { + /** The longer side, corresponds to X values */ public static final Distance kFieldLength = Meters.of(16.541); + /** The shorter side, corresponds to Y values */ public static final Distance kFieldWidth = Meters.of(8.069); - + /** The length of the alliance zone, corresponds to X axis */ + public static final Distance kAllianceZoneLength = Inches.of(182.11); + public static final Pose3d kBlueHub = new Pose3d(4.632516, 4.011139, 1.83, Rotation3d.kZero); + public static final Pose3d kFeedTarget = new Pose3d(4.5, 2, 1.0, Rotation3d.kZero); + } + + public static class AimConstants { + public static final AimStrategy kAim = new PhysicsAim( + new AimConstraints( + Rotation2d.fromDegrees(49.5), // Min pitch + Rotation2d.fromDegrees(72.0), // Max pitch + 18), // Max output (speed) + 2, + 10); } } diff --git a/CompBot/src/main/java/frc/robot/Robot.java b/CompBot/src/main/java/frc/robot/Robot.java index 13cd0a3..612b71a 100644 --- a/CompBot/src/main/java/frc/robot/Robot.java +++ b/CompBot/src/main/java/frc/robot/Robot.java @@ -47,9 +47,9 @@ public Robot() { public void robotPeriodic() { FieldManager.getInstance().clearFuel(); + robotContainer.superstructure.periodic(); StatusSignalUtil.refreshAll(); CommandScheduler.getInstance().run(); - robotContainer.superstructure.periodic(); FieldManager.getInstance().drawFuel(); diff --git a/CompBot/src/main/java/frc/robot/aiming/AimConstraints.java b/CompBot/src/main/java/frc/robot/aiming/AimConstraints.java new file mode 100644 index 0000000..4b6002a --- /dev/null +++ b/CompBot/src/main/java/frc/robot/aiming/AimConstraints.java @@ -0,0 +1,20 @@ +package frc.robot.aiming; + +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * A class representing the physical constraints of the shooter. + */ +public record AimConstraints( + Rotation2d minShooterAngle, + Rotation2d maxShooterAngle, + double maxOutput) { + + /** Returns whether the given aim parameters satisfy this constraint */ + public boolean check(AimParams params) { + boolean outputOk = params.output <= maxOutput(); + boolean pitchOk = (params.pitch.getRadians() >= minShooterAngle.getRadians()) + && (params.pitch.getRadians() <= maxShooterAngle.getRadians()); + return outputOk && pitchOk; + } +} diff --git a/CompBot/src/main/java/frc/robot/aiming/AimMeasurement.java b/CompBot/src/main/java/frc/robot/aiming/AimMeasurement.java index f3499b2..b10321d 100644 --- a/CompBot/src/main/java/frc/robot/aiming/AimMeasurement.java +++ b/CompBot/src/main/java/frc/robot/aiming/AimMeasurement.java @@ -2,12 +2,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; public record AimMeasurement( Distance distance, Rotation2d pitch, - LinearVelocity velocity, + double shooterControl, Time time) { } diff --git a/CompBot/src/main/java/frc/robot/aiming/AimParams.java b/CompBot/src/main/java/frc/robot/aiming/AimParams.java index ad34c85..527a846 100644 --- a/CompBot/src/main/java/frc/robot/aiming/AimParams.java +++ b/CompBot/src/main/java/frc/robot/aiming/AimParams.java @@ -1,17 +1,16 @@ package frc.robot.aiming; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.LinearVelocity; - -import frc.robot.util.OnboardLogger; /** * The parameters of a shooter at a particlar moment in time. This is the type that is returned when * a shot is generated, meaning that this is what is applied to each subystem. */ public class AimParams { + /** The status of the parameters object. */ + public AimStatus status = AimStatus.Unchecked; + /** The manner of speed control the paramters requires */ + public SpeedControl control = SpeedControl.ProjectileVelocity; /** the launch angle of the fuel out of the robot. */ public Rotation2d pitch = Rotation2d.kZero; /** @@ -20,23 +19,56 @@ public class AimParams { * shooter. */ public Rotation2d yaw = Rotation2d.kZero; - /** the velocity that the fuel should be ejected out at, relative to the robot. */ - public LinearVelocity velocity = MetersPerSecond.zero(); + /** + * the output that the fuel should be ejected out at. If control is ProjectileVelocity, then this + * is m/s relative to the robot. + */ + public double output = 0.0; /** the tolerated error in the shot's pitch */ public Rotation2d deltaPitch = Rotation2d.fromDegrees(4); /** the tolerated error in the shot's yaw */ public Rotation2d deltaYaw = Rotation2d.fromDegrees(2); /** the tolerated error in the shot's velocity */ - public LinearVelocity deltaVelocity = MetersPerSecond.of(0.35); + public double deltaOutput = 0.35; + + public AimParams() {} + + public AimParams(AimStatus status) { + this.status = status; + } - private OnboardLogger ologger = new OnboardLogger("AimParams"); + public enum AimStatus { + /** The program has not yet evaluated the valididty of this parameters object */ + Unchecked, + /** Not a possible shot, do not try to attempt. Values are invalid. */ + Impossible, + /** A shot that is calculated to go in */ + Possible; + + public boolean isOk() { + return this == Possible; + } + } + + public static AimParams impossible() { + return new AimParams(AimStatus.Impossible); + } + + /** Returns whether the aim parameters calculated are feasible */ + public boolean isOk() { + return status.isOk(); + } - public AimParams() { - ologger.registerMeasurement("Pitch", () -> pitch.getMeasure(), Degrees); - ologger.registerMeasurement("Yaw", () -> yaw.getMeasure(), Degrees); - ologger.registerMeasurement("Velocity", () -> velocity, MetersPerSecond); - ologger.registerMeasurement("Error/Pitch", () -> deltaPitch.getMeasure(), Degrees); - ologger.registerMeasurement("Error/Yaw", () -> deltaYaw.getMeasure(), Degrees); - ologger.registerMeasurement("Error/Velocity", () -> deltaVelocity, MetersPerSecond); + public enum SpeedControl { + /** + * The velocity parameter of the {@link AimParams} is the projectile's desired velocity, in + * meters per second. + */ + ProjectileVelocity, + /** + * The velocity parameter of the {@link AimParams} is the shooter's control input, in its + * appropriate units. + */ + MechanismControl; } } diff --git a/CompBot/src/main/java/frc/robot/aiming/AimStrategy.java b/CompBot/src/main/java/frc/robot/aiming/AimStrategy.java index f3a2197..2a61b13 100644 --- a/CompBot/src/main/java/frc/robot/aiming/AimStrategy.java +++ b/CompBot/src/main/java/frc/robot/aiming/AimStrategy.java @@ -1,18 +1,15 @@ package frc.robot.aiming; -import frc.robot.superstructure.StateManager; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; /** * An interface representing a method of aim-calculation; i.e. calculating what shot parameters are * necessary for a given robot configuration */ -public abstract class AimStrategy { - public AimParams params = new AimParams(); - +public interface AimStrategy { /** * Updates the AimParams with the new calculated shot based on the robot's state - * - * @param state The {@link frc.robot.superstructure.Superstructure}'s state. */ - public abstract AimParams update(StateManager state); + public AimParams update(Pose3d target, Pose3d shooter, Translation2d velocity); } diff --git a/CompBot/src/main/java/frc/robot/aiming/ExperimentalAim.java b/CompBot/src/main/java/frc/robot/aiming/ExperimentalAim.java deleted file mode 100644 index 4d10990..0000000 --- a/CompBot/src/main/java/frc/robot/aiming/ExperimentalAim.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.aiming; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.superstructure.StateManager; - -public abstract class ExperimentalAim extends AimStrategy { - /** - * Predicts the aim parameters for a static shot from the given position. Velocity should NOT be - * considered in the implementation of this method. - */ - public abstract AimParams predict(StateManager state, AimParams params); - - public final AimParams update(StateManager state) { - params = predict(state, params); - double feulVelocity = params.velocity.in(MetersPerSecond); - // Break up initial velocity - double vx = feulVelocity * params.pitch.getCos() * params.yaw.getCos(); - double vy = feulVelocity * params.pitch.getCos() * params.yaw.getSin(); - double vz = feulVelocity * params.pitch.getSin(); - // Compensate for robot velocity - Translation2d robotVelocity = state.robotVelocity().getTranslation(); - vx -= robotVelocity.getX(); - vy -= robotVelocity.getY(); - // Recalculate velocity - LinearVelocity velocity = MetersPerSecond.of(Math.sqrt(vx * vx + vy * vy + vz * vz)); - // Recalculate angles - Rotation2d yaw = Rotation2d.fromRadians(Math.atan2(vy, vx)); - Rotation2d pitch = Rotation2d.fromRadians(Math.asin(vz / velocity.in(MetersPerSecond))); - // Reapply parameters - params.yaw = yaw; - params.pitch = pitch; - params.velocity = velocity; - return params; - } -} diff --git a/CompBot/src/main/java/frc/robot/aiming/InterpolationAim.java b/CompBot/src/main/java/frc/robot/aiming/InterpolationAim.java deleted file mode 100644 index d275376..0000000 --- a/CompBot/src/main/java/frc/robot/aiming/InterpolationAim.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.aiming; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import java.util.List; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import frc.robot.superstructure.StateManager; - -public class InterpolationAim extends ExperimentalAim { - private final InterpolatingDoubleTreeMap pitchMap; - private final InterpolatingDoubleTreeMap velocityMap; - - public InterpolationAim(List measurements) { - pitchMap = new InterpolatingDoubleTreeMap(); - velocityMap = new InterpolatingDoubleTreeMap(); - for (AimMeasurement measurement : measurements) { - pitchMap.put( - measurement.distance().in(Meters), - measurement.pitch().getRadians()); - velocityMap.put( - measurement.distance().in(Meters), - measurement.velocity().in(MetersPerSecond)); - } - } - - public AimParams predict(StateManager state, AimParams params) { - Translation3d offset = state.aimTarget().getTranslation() - .minus(new Translation3d(state.robotPose().getTranslation())); - double xyDistance = offset.toTranslation2d().getNorm(); - params.velocity = MetersPerSecond.of(velocityMap.get(xyDistance)); - params.pitch = Rotation2d.fromRadians(pitchMap.get(xyDistance)); - params.yaw = Rotation2d.fromRadians(Math.atan2(offset.getY(), offset.getX())); - return params; - } - -} diff --git a/CompBot/src/main/java/frc/robot/aiming/PhysicsAim.java b/CompBot/src/main/java/frc/robot/aiming/PhysicsAim.java index ec4bece..00b2b54 100644 --- a/CompBot/src/main/java/frc/robot/aiming/PhysicsAim.java +++ b/CompBot/src/main/java/frc/robot/aiming/PhysicsAim.java @@ -1,28 +1,107 @@ package frc.robot.aiming; -import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.superstructure.StateManager; - -public class PhysicsAim extends AimStrategy { - private final double finalDescentSpeed; - - public PhysicsAim(double finalDescentSpeed) { - if (finalDescentSpeed <= 0) { - finalDescentSpeed = 1.0; - DriverStation.reportWarning( - "Positive final descent speed required, but nonpositive number found; assigning value of 1.", - false); +import frc.robot.aiming.AimParams.AimStatus; +import frc.robot.aiming.AimParams.SpeedControl; + +public class PhysicsAim implements AimStrategy { + private static final int ITERATIONS = 5; + + private final AimConstraints constraints; + + private final double minDescentVelocity; + private final double maxDescentVelocity; + + public PhysicsAim(AimConstraints constraints, double minDescentVelocity, + double maxDescentVelocity) { + this.constraints = constraints; + this.minDescentVelocity = minDescentVelocity; + this.maxDescentVelocity = maxDescentVelocity; + } + + public AimParams update(Pose3d target, Pose3d shooter, Translation2d shooterVelocity) { + Translation3d offset = target.getTranslation().minus(shooter.getTranslation()); + + // Calculate the pitch values for the minimum and maximum possible v_zf values: + AimParams minParams = quicksolve(offset, shooterVelocity, minDescentVelocity); + AimParams maxParams = quicksolve(offset, shooterVelocity, maxDescentVelocity); + + double minPitch = minParams.pitch.getRadians(); + double maxPitch = maxParams.pitch.getRadians(); + + boolean solutionExists = minPitch <= constraints.maxShooterAngle().getRadians() + && maxPitch >= constraints.minShooterAngle().getRadians(); + + if (!solutionExists) { + return AimParams.impossible(); } - this.finalDescentSpeed = finalDescentSpeed; + + boolean minWorks = constraints.check(minParams); + if (minWorks) { + minParams.status = AimStatus.Possible; + minParams.control = SpeedControl.ProjectileVelocity; + return minParams; + } + + double lower = minDescentVelocity; + double upper = maxDescentVelocity; + + AimParams best = AimParams.impossible(); + + for (int i = 0; i < ITERATIONS; i++) { + double guess = 0.5 * (lower + upper); + AimParams output = quicksolve(offset, shooterVelocity, guess); + boolean ok = constraints.check(output); + if (ok) { + // We're just optimizing, so we won't stop yet. + upper = guess; + best = output; + best.status = AimStatus.Possible; + continue; + } + double pitch = output.pitch.getRadians(); + if (pitch > constraints.maxShooterAngle().getRadians()) { + // Too high + upper = guess; + continue; + } + if (pitch < constraints.minShooterAngle().getRadians()) { + // Too low + lower = guess; + continue; + } + if (!ok) { + // Velocity must exceed the threshold, so let's drop the guess + upper = guess; + } + } + + if (best.status == AimStatus.Impossible) { + return AimParams.impossible(); + } + + // If yaw says to shoot in the wrong direction we don't listen, even if it would work. + Rotation2d towardsTarget = Rotation2d.fromRadians(Math.atan2(offset.getY(), offset.getX())); + double diff = MathUtil.angleModulus(Math.abs(towardsTarget.minus(best.yaw).getRadians())); + if (diff > 0.8 * Math.PI) { // We aren't really pointed at the target. + return AimParams.impossible(); + } + + best.control = SpeedControl.ProjectileVelocity; + return best; } - public AimParams update(StateManager state) { - Translation3d offset = state.aimTarget().getTranslation() - .minus(new Translation3d(state.robotPose().getTranslation())); + public static AimParams quicksolve( + Translation3d offset, + Translation2d robotVelocity, + double finalDescentSpeed) { + + AimParams params = new AimParams(); + double dx = offset.getX(); double dy = offset.getY(); double dz = offset.getZ(); @@ -39,20 +118,19 @@ public AimParams update(StateManager state) { double vy = dy / t; double vz = 9.81 * t - finalDescentSpeed; - // Compensate for robot velocity - Translation2d robotVelocity = state.robotVelocity().getTranslation(); vx -= robotVelocity.getX(); vy -= robotVelocity.getY(); double v = Math.sqrt(vx * vx + vy * vy + vz * vz); - double yaw = Math.atan2(vy, vx); - double pitch = Math.asin(vz / v); + Rotation2d yaw = Rotation2d.fromRadians(Math.atan2(vy, vx)); + Rotation2d pitch = Rotation2d.fromRadians(Math.asin(vz / v)); - params.velocity = MetersPerSecond.of(v); - params.pitch = Rotation2d.fromRadians(pitch); - params.yaw = Rotation2d.fromRadians(yaw); + params.output = v; + params.pitch = pitch; + params.yaw = yaw; return params; } + } diff --git a/CompBot/src/main/java/frc/robot/aiming/PolyRegAim.java b/CompBot/src/main/java/frc/robot/aiming/PolyRegAim.java deleted file mode 100644 index f961c70..0000000 --- a/CompBot/src/main/java/frc/robot/aiming/PolyRegAim.java +++ /dev/null @@ -1,65 +0,0 @@ -package frc.robot.aiming; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import java.util.ArrayList; -import java.util.List; -import org.apache.commons.math3.fitting.PolynomialCurveFitter; -import org.apache.commons.math3.fitting.WeightedObservedPoint; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.superstructure.StateManager; - -public class PolyRegAim extends ExperimentalAim { - private static final int degree = 4; - private static final int maxIterations = Integer.MAX_VALUE; - - private static final PolynomialCurveFitter fitter = PolynomialCurveFitter - .create(degree) - .withMaxIterations(maxIterations); - - private final double[] velocityCoeffs; - private final double[] pitchCoeffs; - - public PolyRegAim(List measurements) { - List velocityData = new ArrayList<>(measurements.size()); - List pitchData = new ArrayList<>(measurements.size()); - for (AimMeasurement measurement : measurements) { - velocityData.add(new WeightedObservedPoint(1, - measurement.distance().in(Meters), - measurement.velocity().in(MetersPerSecond))); - pitchData.add(new WeightedObservedPoint(1, - measurement.distance().in(Meters), - measurement.pitch().getRadians())); - } - velocityCoeffs = fitter.fit(velocityData); - pitchCoeffs = fitter.fit(pitchData); - } - - public AimParams predict(StateManager state, AimParams params) { - Translation3d offset = state.aimTarget().getTranslation() - .minus(new Translation3d(state.robotPose().getTranslation())); - double xyDistance = offset.toTranslation2d().getNorm(); - // Calculate angles and field-relative fuel velocity - Rotation2d yaw = Rotation2d.fromRadians(Math.atan2(offset.getY(), offset.getX())); - Rotation2d pitch = Rotation2d.fromRadians(sample(pitchCoeffs, xyDistance)); - LinearVelocity velocity = MetersPerSecond.of(sample(velocityCoeffs, xyDistance)); - // Apply calculations - params.yaw = yaw; - params.pitch = pitch; - params.velocity = velocity; - return params; - } - - private double sample(double[] coeffs, double x) { - if (coeffs.length != degree + 1) { - return Double.NaN; // there was an error - } - double y = 0; - for (int n = 0; n < degree; n++) { - y += coeffs[n] * Math.pow(x, n); - } - return y; - } -} diff --git a/CompBot/src/main/java/frc/robot/aiming/ToFAim.java b/CompBot/src/main/java/frc/robot/aiming/ToFAim.java new file mode 100644 index 0000000..d995ee6 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/aiming/ToFAim.java @@ -0,0 +1,87 @@ +package frc.robot.aiming; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; +import java.util.List; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import frc.robot.aiming.AimParams.AimStatus; +import frc.robot.aiming.AimParams.SpeedControl; + +/** + * An aim strategy that uses time-of-flight recursion to estimate the ideal shot parameters for the + * robot. This is based on measurements experimentally obtained. + */ +public class ToFAim implements AimStrategy { + static final double EPSILON = 1e-3; + static final int ITERATIONS = 5; + + private final AimConstraints constraints; + + private final InterpolatingDoubleTreeMap timeMap; + private final InterpolatingDoubleTreeMap speedMap; + private final InterpolatingDoubleTreeMap pitchMap; + + public ToFAim(List measurements, AimConstraints constraints) { + this.constraints = constraints; + + timeMap = new InterpolatingDoubleTreeMap(); + speedMap = new InterpolatingDoubleTreeMap(); + pitchMap = new InterpolatingDoubleTreeMap(); + + for (AimMeasurement measurement : measurements) { + double distance = measurement.distance().in(Meters); + double pitch = measurement.pitch().getDegrees(); + double tof = measurement.time().in(Seconds); + timeMap.put(distance, tof); + pitchMap.put(distance, pitch); + speedMap.put(distance, measurement.shooterControl()); + } + } + + public AimParams update(Pose3d aimTarget, Pose3d shooterPose, Translation2d shooterVelocity) { + Translation2d target = aimTarget.getTranslation().toTranslation2d(); + Translation2d start = shooterPose.getTranslation().toTranslation2d(); + + Translation2d afterShooting = start; + + AimStatus status = AimStatus.Impossible; + + double distance = 0.0; // This will be overriden immediately + double tof; + + for (int i = 0; i < ITERATIONS; i++) { + // Predict the ToF for the current shot + distance = afterShooting.minus(target).getNorm(); + tof = timeMap.get(distance); + Translation2d newAfterShooting = start.plus(shooterVelocity.times(tof)); + double error = newAfterShooting.minus(afterShooting).getNorm(); + if (error < EPSILON) { + // Solution found! + status = AimStatus.Possible; + break; + } + + afterShooting = newAfterShooting; + } + + if (status == AimStatus.Impossible) { + return AimParams.impossible(); + } + + double pitch = pitchMap.get(distance); + double shooterControl = speedMap.get(distance); + + AimParams params = new AimParams(); + params.pitch = Rotation2d.fromDegrees(pitch); + params.output = shooterControl; + params.control = SpeedControl.MechanismControl; + Translation2d finalOffset = target.minus(afterShooting); + params.yaw = Rotation2d.fromRadians(Math.atan2(finalOffset.getY(), finalOffset.getX())); + + params.status = (constraints.check(params)) ? AimStatus.Possible : AimStatus.Impossible; + return params; + } +} diff --git a/CompBot/src/main/java/frc/robot/commands/AimTrack.java b/CompBot/src/main/java/frc/robot/commands/AimTrack.java index 686bfa8..f1466e2 100644 --- a/CompBot/src/main/java/frc/robot/commands/AimTrack.java +++ b/CompBot/src/main/java/frc/robot/commands/AimTrack.java @@ -8,7 +8,7 @@ public class AimTrack implements CommandBuilder { public Command build(Subsystems subsystems, StateManager state) { return Commands.parallel( - subsystems.turret().track(state, state::aimParams), - subsystems.shooter().shoot(state::aimParams)); + subsystems.turret().track(state), + subsystems.shooter().shoot(state::predictedAimParams)); } } diff --git a/CompBot/src/main/java/frc/robot/commands/FuelShotSim.java b/CompBot/src/main/java/frc/robot/commands/FuelShotSim.java index 5b6081e..683b05e 100644 --- a/CompBot/src/main/java/frc/robot/commands/FuelShotSim.java +++ b/CompBot/src/main/java/frc/robot/commands/FuelShotSim.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldManager; import frc.robot.aiming.AimParams; +import frc.robot.aiming.AimParams.SpeedControl; import frc.robot.superstructure.StateManager; import frc.robot.superstructure.Superstructure.Subsystems; @@ -15,16 +16,15 @@ public class FuelShotSim implements CommandBuilder { public Command singleBuild(Subsystems subsystems, StateManager state) { FuelSim sim = new FuelSim(); return Commands.sequence( - Commands.runOnce(() -> { - sim.launch(state, state.aimParams()); - }), + Commands.runOnce(() -> sim.launch(state)), Commands.run(sim::tick)) .until(sim::atHub) .withName("Fuel Shot (sim)"); } public Command build(Subsystems subsystems, StateManager state) { - return Commands.runOnce(() -> CommandScheduler.getInstance().schedule(singleBuild(subsystems, state))); + return Commands + .runOnce(() -> CommandScheduler.getInstance().schedule(singleBuild(subsystems, state))); } public class FuelSim { @@ -39,27 +39,33 @@ public class FuelSim { public FuelSim() {} - public void launch(StateManager state, AimParams params) { - position = new Translation3d(state.robotPose().getTranslation()); + public void launch(StateManager state) { + AimParams params = state.predictedAimParams(); + if (params.control != SpeedControl.ProjectileVelocity) { + throw new IllegalStateException( + "Aim Params in fuel sim doesn't use SpeedControl.ProjectileVelocity speed control"); + } + position = state.turretPose().getTranslation(); final double error = 0.15; // field-relative velocity, but with the robot as the origin Translation3d veloR = new Translation3d( - params.pitch.getCos() * params.yaw.getCos() * params.velocity.baseUnitMagnitude() + Math.random() * error, - params.pitch.getCos() * params.yaw.getSin() * params.velocity.baseUnitMagnitude() + Math.random() * error, - params.pitch.getSin() * params.velocity.baseUnitMagnitude() + Math.random() * error); + params.pitch.getCos() * params.yaw.getCos() * params.output + + Math.random() * error, + params.pitch.getCos() * params.yaw.getSin() * params.output + + Math.random() * error, + params.pitch.getSin() * params.output + Math.random() * error); velocity = veloR.plus(new Translation3d(state.robotVelocity().getTranslation())); target = state.aimTarget().getTranslation(); } public void tick() { - for (int i = 0;i < resolution;i ++) { + for (int i = 0; i < resolution; i++) { // Update position - position = position.plus(velocity.times(dt / (double)resolution)); - // Update velocity + position = position.plus(velocity.times(dt / (double) resolution)); // Calculate the drag force / mass: 0.5 * 1.225 * pi * 0.075^2 * 0.47 * v^2 * 2 double Fd = -0.5 * 1.225 * Math.PI * 0.075 * 0.075 * 0.47 * velocity.getNorm() * 2; Translation3d acceleration = velocity.times(Fd).plus(gravity); - velocity = velocity.plus(acceleration.times(dt / (double)resolution)); + velocity = velocity.plus(acceleration.times(dt / (double) resolution)); } FieldManager.getInstance().addFuel(new Pose3d(position, Rotation3d.kZero)); } diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 783ded0..7779804 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -21,6 +21,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -153,7 +154,7 @@ public Drivetrain( poseLogEntry = StructLogEntry.create(DataLogManager.getLog(), "Robot Pose", Pose2d.struct); state = getState(); OnboardLogger ologger = new OnboardLogger("Drivetrain"); - ologger.registerBoolean("Received vision udpate", () -> hasReceivedVisionUpdate); + ologger.registerBoolean("Received vision update", () -> hasReceivedVisionUpdate); } /** @@ -323,4 +324,20 @@ public Command rotate() { return this.applyRequest( () -> new SwerveRequest.RobotCentric().withRotationalRate(0.5 * Math.PI)); } + + private Twist2d predictedTwist() { + return new Twist2d( + state.Speeds.vxMetersPerSecond * Robot.kDefaultPeriod, + state.Speeds.vyMetersPerSecond * Robot.kDefaultPeriod, + state.Speeds.omegaRadiansPerSecond * Robot.kDefaultPeriod); + } + + public Pose2d predictedRobotPose() { + return robotPose().exp(predictedTwist()); + } + + public Translation2d predictedRobotVelocity() { + return robotVelocity().getTranslation().rotateBy( + Rotation2d.fromRadians(state.Speeds.omegaRadiansPerSecond * Robot.kDefaultPeriod)); + } } diff --git a/CompBot/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/CompBot/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 7088081..dfadaf3 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/CompBot/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -8,7 +9,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; @@ -44,10 +44,10 @@ public void periodic() { io.updateInputs(inputs); } - private AngularVelocity projectileToShooterVelocity(LinearVelocity projectileVelocity) { + private AngularVelocity projectileToShooterVelocity(double projectileVelocity) { // Assume linear relationship between shooter rotational speed and projectile linear speed. return ShooterConstants.kMaxRotationalSpeed - .times(projectileVelocity.div(ShooterConstants.kMaxLinearSpeed)); + .times(projectileVelocity / ShooterConstants.kMaxLinearSpeed.in(MetersPerSecond)); } private Angle pitchToHoodAngle(Rotation2d pitch) { @@ -62,7 +62,7 @@ private Angle pitchToHoodAngle(Rotation2d pitch) { */ public Command shoot(Supplier params) { return this.run(() -> { - shooterReference = projectileToShooterVelocity(params.get().velocity); + shooterReference = projectileToShooterVelocity(params.get().output); hoodReference = pitchToHoodAngle(params.get().pitch); io.setVelocity(shooterReference); io.setAngle(hoodReference); @@ -79,9 +79,9 @@ public Command reverse() { public Trigger tracked(Supplier params) { return new Trigger(() -> { double velocityError = inputs.shooter1Velocity - .minus(projectileToShooterVelocity(params.get().velocity)).baseUnitMagnitude(); + .minus(projectileToShooterVelocity(params.get().output)).baseUnitMagnitude(); boolean velocityOk = - Math.abs(velocityError) <= params.get().deltaVelocity.baseUnitMagnitude(); + Math.abs(velocityError) <= params.get().deltaOutput; double hoodError = inputs.hoodPosition.minus(pitchToHoodAngle(params.get().pitch)).baseUnitMagnitude(); diff --git a/CompBot/src/main/java/frc/robot/subsystems/turret/Turret.java b/CompBot/src/main/java/frc/robot/subsystems/turret/Turret.java index c9e7958..9afd24b 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/CompBot/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,14 +1,17 @@ package frc.robot.subsystems.turret; import static edu.wpi.first.units.Units.Revolutions; - +import static edu.wpi.first.units.Units.Rotations; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -54,15 +57,21 @@ public void periodic() { * Returns a {@link Command} that will continually attempt to listen to the {@link AimParams} * object that is supplied, accounting for the robot's rotation. */ - public Command track(StateManager state, Supplier aimParams) { + public Command track(StateManager state) { return Commands.sequence( this.run(() -> { tracking = true; Rotation2d robot = state.robotPose().getRotation(); - Rotation2d relative = aimParams.get().yaw.minus(robot); - io.setPosition(relative.getMeasure().plus(TurretConstants.kTrackingOffset)); + Rotation2d relative = state.predictedAimParams().yaw.minus(robot); + // We're only in "tracking" mode if we're just trying to get to a happy spot. If everybody + // else is ready, we don't want to hold up shooting, so we allow the turret access to its + // full range. We don't generally want to do this, because it would mean that while + // shooting, we would be more likely to hit the turret's physical max and *force* + // ourselves to rotate the turret all the way around... nonideal. + setPosition(relative.getMeasure().plus(TurretConstants.kForwards), + !state.shootReady().getAsBoolean()); })) - .finallyDo(() -> tracking = false); + .finallyDo(() -> tracking = false); } /** @@ -70,13 +79,13 @@ public Command track(StateManager state, Supplier aimParams) { */ public Command home() { return Commands.sequence( - runOnce(() -> io.setPosition(TurretConstants.kHomePosition)), + runOnce(() -> setPosition(TurretConstants.kHomePosition, false)), Commands.waitUntil(ready())); } public Command forwards() { return Commands.sequence( - runOnce(() -> io.setPosition(TurretConstants.kTrackingOffset)), + runOnce(() -> setPosition(TurretConstants.kForwards, false)), Commands.waitUntil(ready())); } @@ -102,11 +111,75 @@ public Trigger tracked(Supplier params) { public void telemetrize(StateManager state) { Pose2d turretPosition = state.robotPose().transformBy(TurretConstants.kTurretPosition.plus( new Transform2d(Translation2d.kZero, - new Rotation2d(inputs.position.minus(TurretConstants.kTrackingOffset))))); + new Rotation2d(inputs.position.minus(TurretConstants.kForwards))))); Pose2d turretReference = state.robotPose().transformBy(TurretConstants.kTurretPosition.plus( new Transform2d(Translation2d.kZero, - new Rotation2d(inputs.reference.minus(TurretConstants.kTrackingOffset))))); + new Rotation2d(inputs.reference.minus(TurretConstants.kForwards))))); FieldManager.getInstance().getField().getObject("turret").setPose(turretPosition); FieldManager.getInstance().getField().getObject("turret-target").setPose(turretReference); } + + public Pose3d turretPose(Pose2d robotPose) { + return new Pose3d(robotPose).transformBy(TurretConstants.kOffset); + } + + /** + * This algorithm calculates the "ideal" position for the turret to rotate through. + */ + private void setPosition(Angle position, boolean tracking) { + Angle min = (tracking) ? TurretConstants.kMinTrackingAngle : TurretConstants.kMinAngle; + Angle max = (tracking) ? TurretConstants.kMaxTrackingAngle : TurretConstants.kMaxAngle; + io.setPosition(Rotations.of(findCC( + inputs.position.in(Rotations), + position.in(Rotations), + min.in(Rotations), + max.in(Rotations)))); + } + + /** + * Returns the Closest Conguent value in the range [min,max], modulo 1 + * + * WARNING: this only works if you're SURE that max - min >= 1 + * + * @param position the current, non-wrapped position + * @param reference the goal position, in the range [0,1] + * @param min the minimum position + * @param max the maximum position + */ + protected static double findCC(double position, double reference, double min, double max) { + if (max - min < 1.0) { + DriverStation.reportError( + "Invalid range passed to Turret.findCC: " + min + " to " + max, + true); + return position; + } + // Ensure reference is already a valid reference + while (reference > max) { + reference -= 1.0; + } + while (reference < min) { + reference += 1.0; + } + double error = Math.abs(reference - position); + if (Math.abs(error) < 0.5) { + return reference; + } + double offset = (reference > position) ? -1 : 1; + while (true) { + double newReference = reference + offset; + if (newReference > max || newReference < min) { + break; + } + double newError = Math.abs(newReference - position); + if (newError > error) { + break; + } + if (Math.abs(newError) < 0.5) { + // This is the closest we'll get + return newReference; + } + reference = newReference; + } + return reference; + } } diff --git a/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 8ca865a..d92a3c1 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -1,7 +1,9 @@ package frc.robot.subsystems.turret; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Revolutions; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -14,7 +16,9 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.units.measure.Angle; public class TurretConstants { @@ -32,7 +36,7 @@ public class TurretConstants { * robot. For example, if this value was 30 degrees, then setting the turret's position to 30 * degrees would result in the turret pointing forwards on the robot. */ - protected static final Angle kTrackingOffset = Revolutions.of(0.25); + protected static final Angle kForwards = Revolutions.of(0.25); // MotionMagic configuration protected static final double kGearRatio = 38.46; @@ -88,4 +92,16 @@ public class TurretConstants { .withMotionMagicAcceleration(kMaxAcceleration) .withMotionMagicJerk(kMaxJerk)); + /** The turret's relative position on the robot */ + protected static final Transform3d kOffset = new Transform3d( + Inches.of(-4.4), + Inches.of(4.4), + Inches.of(22.5), + Rotation3d.kZero); + + // These parameters define the range of valid angles for the turret + protected static final Angle kMinAngle = Rotations.of(-0.75); + protected static final Angle kMinTrackingAngle = Rotations.of(-0.5); + protected static final Angle kMaxAngle = Rotations.of(0.75); + protected static final Angle kMaxTrackingAngle = Rotations.of(0.5); } diff --git a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java index 8692eee..ebebd73 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java +++ b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java @@ -1,12 +1,17 @@ package frc.robot.superstructure; +import static edu.wpi.first.units.Units.Degrees; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.AimConstants; +import frc.robot.aiming.AimConstraints; import frc.robot.aiming.AimParams; import frc.robot.aiming.AimStrategy; import frc.robot.aiming.PhysicsAim; +import frc.robot.aiming.AimParams.AimStatus; import frc.robot.superstructure.Superstructure.Subsystems; import frc.robot.util.FieldUtils; import frc.robot.util.OnboardLogger; @@ -16,15 +21,41 @@ */ public class StateManager { private final Subsystems subsystems; - public final AimStrategy aim = new PhysicsAim(2.0); + + private AimParams params = new AimParams(AimStatus.Unchecked); + private AimParams predictedParams = new AimParams(AimStatus.Unchecked); public StateManager(Subsystems subsystems) { this.subsystems = subsystems; - OnboardLogger log = new OnboardLogger("Robot State"); + OnboardLogger log = new OnboardLogger("Robot"); log.registerPose("Robot Pose", this::robotPose); log.registerTransform2d("Robot Velocity", this::robotVelocity); log.registerPose3d("Aim Target", this::aimTarget); + log.registerPose3d("Turret Position", this::turretPose); log.registerBoolean("Shoot Ready", shootReady()); + log.registerBoolean("Turret Tracked", subsystems.turret().tracked(this::aimParams)); + log.registerBoolean("Shooter Tracked", subsystems.shooter().tracked(this::aimParams)); + + String aimPrefix = "Aim Params/"; + log.registerString(aimPrefix + "Status", () -> params.status.toString()); + log.registerMeasurement(aimPrefix + "Pitch", () -> params.pitch.getMeasure(), Degrees); + log.registerMeasurement(aimPrefix + "Yaw", () -> params.yaw.getMeasure(), Degrees); + log.registerDouble(aimPrefix + "Velocity", () -> params.output); + log.registerMeasurement(aimPrefix + "Error/Pitch", () -> params.deltaPitch.getMeasure(), + Degrees); + log.registerMeasurement(aimPrefix + "Error/Yaw", () -> params.deltaYaw.getMeasure(), Degrees); + log.registerDouble(aimPrefix + "Error/Velocity", () -> params.deltaOutput); + + aimPrefix = "Aim Params (Predicted)/"; + log.registerString(aimPrefix + "Status", () -> predictedParams.status.toString()); + log.registerMeasurement(aimPrefix + "Pitch", () -> predictedParams.pitch.getMeasure(), Degrees); + log.registerMeasurement(aimPrefix + "Yaw", () -> predictedParams.yaw.getMeasure(), Degrees); + log.registerDouble(aimPrefix + "Velocity", () -> predictedParams.output); + log.registerMeasurement(aimPrefix + "Error/Pitch", + () -> predictedParams.deltaPitch.getMeasure(), Degrees); + log.registerMeasurement(aimPrefix + "Error/Yaw", () -> predictedParams.deltaYaw.getMeasure(), + Degrees); + log.registerDouble(aimPrefix + "Error/Velocity", () -> predictedParams.deltaOutput); } /** @@ -42,21 +73,44 @@ public Transform2d robotVelocity() { } public Pose3d aimTarget() { - // For now, just assume that we're targeting the nearest hub. - // TODO: add feeding logic as well. - return FieldUtils.hub(); + if (FieldUtils.inAllianceZone(robotPose())) { + return FieldUtils.hub(); + } else { + return FieldUtils.feedTarget(); + } } public Trigger shootReady() { return subsystems.turret().tracked(this::aimParams) - .and(subsystems.shooter().tracked(this::aimParams)); + .and(subsystems.shooter().tracked(this::aimParams)) + .and(() -> params.isOk()); } public AimParams aimParams() { - return aim.params; + if (params.status == AimStatus.Unchecked) { + params = + AimConstants.kAim.update(aimTarget(), turretPose(), robotVelocity().getTranslation()); + } + return params; + } + + public AimParams predictedAimParams() { + if (predictedParams.status == AimStatus.Unchecked) { + Pose2d predictedPose = subsystems.drivetrain().predictedRobotPose(); + predictedParams = + AimConstants.kAim.update(aimTarget(), subsystems.turret().turretPose(predictedPose), + subsystems.drivetrain().predictedRobotVelocity()); + } + return predictedParams; } public void periodic() { - aim.update(this); // This ensures we only cache the parameter object and then cache them. + params = new AimParams(AimStatus.Unchecked); + predictedParams = new AimParams(AimStatus.Unchecked); + } + + public Pose3d turretPose() { + return subsystems.turret().turretPose(robotPose()); } + } diff --git a/CompBot/src/main/java/frc/robot/util/FieldUtils.java b/CompBot/src/main/java/frc/robot/util/FieldUtils.java index 4bac0c0..719e850 100644 --- a/CompBot/src/main/java/frc/robot/util/FieldUtils.java +++ b/CompBot/src/main/java/frc/robot/util/FieldUtils.java @@ -1,7 +1,10 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants.FieldConstants; @@ -13,23 +16,31 @@ public class FieldUtils { private static Pose3d allianceRelativeFlip(Pose3d pose) { if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) { - return null; + return new Pose3d( + FieldConstants.kFieldLength.minus(pose.getMeasureX()), + FieldConstants.kFieldWidth.minus(pose.getMeasureY()), + pose.getMeasureZ(), + pose.getRotation().rotateBy(new Rotation3d(Rotation2d.kPi))); } else { return pose; } } private static Pose2d allianceRelativeFlip(Pose2d pose) { - if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) { - return null; - } else { - return pose; - } + return allianceRelativeFlip(new Pose3d(pose)).toPose2d(); } /** Returns the position of the hub corresponding to the currently selected alliance */ - // TODO: Actually flip target based on alliance public static Pose3d hub() { - return FieldConstants.kBlueHub; + return allianceRelativeFlip(FieldConstants.kBlueHub); + } + + public static boolean inAllianceZone(Pose2d robot) { + Pose2d local = allianceRelativeFlip(robot); + return local.getX() <= FieldConstants.kAllianceZoneLength.in(Meters); + } + + public static Pose3d feedTarget() { + return allianceRelativeFlip(FieldConstants.kFeedTarget); } } diff --git a/CompBot/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java b/CompBot/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java index 4933b9c..b52b155 100644 --- a/CompBot/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java +++ b/CompBot/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java @@ -202,9 +202,9 @@ private boolean isOutsideField(Pose3d pose) { double y = pose.getY(); double z = pose.getZ(); double xMax = LocalizationConstants.kXYMargin.magnitude() - + FieldConstants.kFieldWidth.magnitude(); - double yMax = LocalizationConstants.kXYMargin.magnitude() + FieldConstants.kFieldLength.magnitude(); + double yMax = LocalizationConstants.kXYMargin.magnitude() + + FieldConstants.kFieldWidth.magnitude(); double xyMin = -LocalizationConstants.kXYMargin.magnitude(); double zMax = LocalizationConstants.kZMargin.magnitude(); double zMin = -LocalizationConstants.kZMargin.magnitude(); diff --git a/CompBot/src/test/java/frc/robot/aiming/PhysicsAimTest.java b/CompBot/src/test/java/frc/robot/aiming/PhysicsAimTest.java new file mode 100644 index 0000000..81b58af --- /dev/null +++ b/CompBot/src/test/java/frc/robot/aiming/PhysicsAimTest.java @@ -0,0 +1,44 @@ +package frc.robot.aiming; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; +import org.junit.jupiter.api.Test; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.aiming.AimParams.AimStatus; + +public class PhysicsAimTest { + @Test + public void setStatusTest() { + AimParams params = new AimParams(); + assertEquals(params.status, AimStatus.Unchecked); + params.status = AimStatus.Possible; + assertEquals(params.status, AimStatus.Possible); + assertTrue(params.isOk()); + params.status = AimStatus.Impossible; + assertEquals(params.status, AimStatus.Impossible); + } + + @Test + public void ensureMonotonic() { + Translation3d offset = new Translation3d(1,1,3); + Translation2d velocity = Translation2d.kZero; + double last = Double.NEGATIVE_INFINITY; + for (double vz = 0.0; vz <= 5.0; vz += 0.1) { + AimParams params = PhysicsAim.quicksolve(offset, velocity, vz); + assertTrue(params.pitch.getDegrees() > last); + last = params.pitch.getDegrees(); + } + } + + @Test + public void testAngleRounding() { + Rotation2d angle1 = Rotation2d.fromDegrees(180); + Rotation2d angle2 = Rotation2d.fromDegrees(-180); + Rotation2d difference = angle1.minus(angle2); + double diff = MathUtil.angleModulus(difference.getRadians()); + assertTrue(Math.abs(diff) < 1e-3); + } +} diff --git a/CompBot/src/test/java/frc/robot/subsystems/shooter/ShooterTest.java b/CompBot/src/test/java/frc/robot/subsystems/shooter/ShooterTest.java index 388c180..675a1eb 100644 --- a/CompBot/src/test/java/frc/robot/subsystems/shooter/ShooterTest.java +++ b/CompBot/src/test/java/frc/robot/subsystems/shooter/ShooterTest.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.MetersPerSecond; import static org.junit.jupiter.api.Assertions.*; import static org.mockito.Mockito.doAnswer; import static org.mockito.Mockito.mock; @@ -34,13 +35,13 @@ public void shooterTest() { AimParams params = new AimParams(); params.pitch = Rotation2d.fromDegrees(55); - params.velocity = ShooterConstants.kMaxLinearSpeed; + params.output = ShooterConstants.kMaxLinearSpeed.in(MetersPerSecond); CommandScheduler.getInstance().schedule(shooter.shoot(() -> params)); CommandScheduler.getInstance().run(); // These methods should have been called from the running command. verify(mockShooterIO).setVelocity(ShooterConstants.kMaxRotationalSpeed - .times(params.velocity.div(ShooterConstants.kMaxLinearSpeed))); + .times(params.output / ShooterConstants.kMaxLinearSpeed.in(MetersPerSecond))); verify(mockShooterIO).setAngle(Degrees.of(35).minus(HoodConstants.kOffset)); CommandScheduler.getInstance().schedule(shooter.reverse()); diff --git a/CompBot/src/test/java/frc/robot/subsystems/turret/TurretTest.java b/CompBot/src/test/java/frc/robot/subsystems/turret/TurretTest.java new file mode 100644 index 0000000..ca7cf41 --- /dev/null +++ b/CompBot/src/test/java/frc/robot/subsystems/turret/TurretTest.java @@ -0,0 +1,70 @@ +package frc.robot.subsystems.turret; + +import static edu.wpi.first.units.Units.Degrees; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.verify; +import static org.mockito.Mockito.when; +import org.junit.jupiter.api.Test; +import org.mockito.Mockito; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.CommandBasedTest; +import frc.robot.aiming.AimParams; +import frc.robot.aiming.AimParams.AimStatus; +import frc.robot.subsystems.turret.TurretIO.TurretIOInputs; +import frc.robot.superstructure.StateManager; + +public class TurretTest extends CommandBasedTest { + @Test + public void turretTest() { + TurretIO mockIO = mock(TurretIO.class); + + Turret turret = new Turret(mockIO); + verify(mockIO).calibrate(); + + CommandScheduler.getInstance().run(); + verify(mockIO).updateInputs(Mockito.any(TurretIOInputs.class)); + + CommandScheduler.getInstance().schedule(turret.forwards()); + verify(mockIO).setPosition(TurretConstants.kForwards); + + StateManager mockState = mock(StateManager.class); + AimParams params = new AimParams(AimStatus.Possible); + params.yaw = Rotation2d.fromDegrees(34.14); + when(mockState.predictedAimParams()).thenReturn(params); + when(mockState.robotPose()).thenReturn(new Pose2d(Translation2d.kZero, Rotation2d.fromDegrees(10.0))); + when(mockState.shootReady()).thenReturn(new Trigger(() -> false)); + + CommandScheduler.getInstance().schedule(turret.track(mockState)); + CommandScheduler.getInstance().run(); + + // The turret should have adjusted to the robot's heading to make sure that the field-relative + // angle is correct. + verify(mockIO).setPosition(Degrees.of(24.14).plus(TurretConstants.kForwards)); + } + + @Test + public void angleWrapTest() { + double[][] testCases = { + {0, 0, -0.75, 0.75, 0}, + {0, 1.1, 0, 2, 0.1}, + {1, 0.9, 0.95, 2, 1.9}}; + + for (double[] testCase: testCases) { + double x, y, min, max, expected; + x = testCase[0]; + y = testCase[1]; + min = testCase[2]; + max = testCase[3]; + expected = testCase[4]; + + double out = Turret.findCC(x, y, min, max); + assertTrue(Math.abs(out - expected) < 1e-3); + } + } + +}