diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml index e80bcac0..cd3acb37 100644 --- a/.idea/codeStyles/Project.xml +++ b/.idea/codeStyles/Project.xml @@ -3,6 +3,9 @@ \ No newline at end of file diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 042351a4..39d87894 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 042351a460b9f5654e60b310a9266fbebe4ba53b +Subproject commit 39d87894e001e230878dba6f75b32787d85c71fc diff --git a/src/main/java/competition/commandgroups/DriveToFaceAndScoreCommandGroupFactory.java b/src/main/java/competition/commandgroups/DriveToFaceAndScoreCommandGroupFactory.java index 70fad7d7..c4032e49 100644 --- a/src/main/java/competition/commandgroups/DriveToFaceAndScoreCommandGroupFactory.java +++ b/src/main/java/competition/commandgroups/DriveToFaceAndScoreCommandGroupFactory.java @@ -45,7 +45,8 @@ public DriveToFaceAndScoreCommandGroupFactory(DriveToReefFaceThenAlignCommandGro public SequentialCommandGroup create(Landmarks.ReefFace targetReefFace, Landmarks.Branch targetBranch, - Landmarks.CoralLevel targetLevel) { + Landmarks.CoralLevel targetLevel, + boolean useSplinesForRouting) { // Overarching command group — drives to branch and preps coral system once the robot is close enough to the reef, scores when ready var driveToFaceAndScoreCommandGroup = new SequentialCommandGroup(); @@ -53,7 +54,7 @@ public SequentialCommandGroup create(Landmarks.ReefFace targetReefFace, var driveToBranchWhilePrepping = new ParallelCommandGroup(); // Terminally approach to branch - var driveToReefFaceThenAlign = driveToReefFaceThenAlignCommandGroupFactory.create(targetReefFace, targetBranch); + var driveToReefFaceThenAlign = driveToReefFaceThenAlignCommandGroupFactory.create(targetReefFace, targetBranch, useSplinesForRouting); // Prep coral system once robot is within a distance threshold var measureDistanceThenPrep = new SequentialCommandGroup(); @@ -79,4 +80,10 @@ public SequentialCommandGroup create(Landmarks.ReefFace targetReefFace, return driveToFaceAndScoreCommandGroup; } + public SequentialCommandGroup create(Landmarks.ReefFace targetReefFace, + Landmarks.Branch targetBranch, + Landmarks.CoralLevel targetLevel) { + return create(targetReefFace, targetBranch, targetLevel, false); + } + } diff --git a/src/main/java/competition/commandgroups/DriveToReefFaceThenAlignCommandGroupFactory.java b/src/main/java/competition/commandgroups/DriveToReefFaceThenAlignCommandGroupFactory.java index 00b1ab2e..d94a2d5b 100644 --- a/src/main/java/competition/commandgroups/DriveToReefFaceThenAlignCommandGroupFactory.java +++ b/src/main/java/competition/commandgroups/DriveToReefFaceThenAlignCommandGroupFactory.java @@ -1,16 +1,14 @@ package competition.commandgroups; import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.drive.commands.AlignToReefWithAprilTagCommand; import competition.subsystems.drive.commands.AlignToTagGlobalMovementWithCalculator; -import competition.subsystems.drive.commands.DriveToReefFaceUntilDetectionCommand; +import competition.subsystems.drive.commands.DriveHermiteSplineCommand; import competition.subsystems.drive.logic.AlignCameraToAprilTagCalculator; import competition.subsystems.pose.Cameras; import competition.subsystems.pose.Landmarks; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; import edu.wpi.first.wpilibj2.command.DeferredCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import xbot.common.controls.sensors.XXboxController; import javax.inject.Inject; import javax.inject.Provider; @@ -18,45 +16,52 @@ public class DriveToReefFaceThenAlignCommandGroupFactory { - Provider alignToReefWithAprilTagCommandProvider; + final Provider alignToReefWithAprilTagCommandProvider; + final Provider splineDriveFactory; AprilTagVisionSubsystemExtended aprilTagVisionSubsystem; DriveSubsystem drive; @Inject public DriveToReefFaceThenAlignCommandGroupFactory(Provider alignToReefWithAprilTagCommandProvider, + Provider splineDriveFactory, AprilTagVisionSubsystemExtended aprilTagVisionSubsystem, DriveSubsystem drive) { this.alignToReefWithAprilTagCommandProvider = alignToReefWithAprilTagCommandProvider; + this.splineDriveFactory = splineDriveFactory; this.aprilTagVisionSubsystem = aprilTagVisionSubsystem; this.drive = drive; } public void setBranch(AlignToTagGlobalMovementWithCalculator command, Landmarks.ReefFace reefFace, Landmarks.Branch branch) { + int cameraIndex = 0; if (branch == Landmarks.Branch.A) { - command.setConfigurations(Cameras.FRONT_RIGHT_CAMERA.getIndex(), - aprilTagVisionSubsystem.getTargetAprilTagID(reefFace), false, 1, - AlignCameraToAprilTagCalculator.Activity.ApproachWhileCentering, false); + cameraIndex = Cameras.FRONT_RIGHT_CAMERA.getIndex(); } else { - command.setConfigurations(Cameras.FRONT_LEFT_CAMERA.getIndex(), - aprilTagVisionSubsystem.getTargetAprilTagID(reefFace), false, 1, - AlignCameraToAprilTagCalculator.Activity.ApproachWhileCentering, false); + cameraIndex = Cameras.FRONT_LEFT_CAMERA.getIndex(); } + + command.setConfigurations(cameraIndex, + () -> aprilTagVisionSubsystem.getTargetAprilTagID(reefFace), false, 1, + AlignCameraToAprilTagCalculator.Activity.ApproachWhileCentering, false); } public SequentialCommandGroup create(Landmarks.ReefFace targetReefFace, Landmarks.Branch targetBranch) { + return create(targetReefFace, targetBranch, false); + } + + public SequentialCommandGroup create(Landmarks.ReefFace targetReefFace, Landmarks.Branch targetBranch, boolean useSplinesForRouting) { var group = new SequentialCommandGroup(); + if (useSplinesForRouting) { + var splineDriveCommand = splineDriveFactory.get(); + splineDriveCommand.configureForReef(targetReefFace, targetBranch); + group.addCommands(splineDriveCommand); + } - var alignToReefCommand = new DeferredCommand( - () -> { - var alignToReefWithAprilTagCommand = alignToReefWithAprilTagCommandProvider.get(); - setBranch(alignToReefWithAprilTagCommand, targetReefFace, targetBranch); - return alignToReefWithAprilTagCommand; - }, Set.of(drive) - ); - group.addCommands( - alignToReefCommand); + var alignToReefWithAprilTagCommand = alignToReefWithAprilTagCommandProvider.get(); + setBranch(alignToReefWithAprilTagCommand, targetReefFace, targetBranch); + group.addCommands(alignToReefWithAprilTagCommand); return group; } } diff --git a/src/main/java/competition/motion/CommonRouteGenerator.java b/src/main/java/competition/motion/CommonRouteGenerator.java new file mode 100644 index 00000000..fb95211c --- /dev/null +++ b/src/main/java/competition/motion/CommonRouteGenerator.java @@ -0,0 +1,240 @@ +package competition.motion; + +import competition.subsystems.oracle.ReefCoordinateGenerator; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.PoseSubsystem; +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.wpilibj.DriverStation; + +import javax.inject.Inject; +import javax.inject.Singleton; +import java.util.ArrayList; +import java.util.List; + +import static edu.wpi.first.units.Units.Meters; + +@Singleton +public class CommonRouteGenerator { + + final ReefCoordinateGenerator reefCoordinateGenerator; + final PoseSubsystem pose; + + @Inject + public CommonRouteGenerator(ReefCoordinateGenerator reefCoordinateGenerator, PoseSubsystem pose) { + this.reefCoordinateGenerator = reefCoordinateGenerator; + this.pose = pose; + } + + // We're mostly interested in routes from the reef to the loading station and vice versa. + // For going to the reef, we'll aim to be pointed at the reef with some small final velocity + // relatively close so vision can do the final approach. + // For going to the coral station, we'll just go straight there with a ~1 foot offset and modest velocity + // and rely on a final shove for alignment + + Translation2d pointJustLeftOfReef = new Translation2d(4.5, 5.7); + Translation2d pointJustAboveReef = new Translation2d(6.2, 4); + + public ArrayList getRouteFromReefToLoadingStation( + Landmarks.ReefFace startingFace, + Landmarks.CoralStation endingStation) { + + // All routes are tuned for blue alliance going to left coral station. If we want to go to the right coral station, + // we mirror our starting face and get a route to left coral station, then mirror the whole route to get one that + // would go to the right coral station. + // Then, if we are on Red alliance, we field rotate the entire spline set. + + var currentPose = pose.getCurrentPose2d(); + + if (endingStation == Landmarks.CoralStation.RIGHT) { + startingFace = Landmarks.mirrorReefFaceLeftToRight(startingFace); + // Pretend the robot is on the other side as well. + currentPose = new Pose2d( + new Translation2d(currentPose.getX(), PoseSubsystem.mirrorYCoordinateAcrossMidfield(currentPose.getY())), + currentPose.getRotation().times(-1)); + } + + var splines = new ArrayList(); + var loadingStationEndPose = getLeftLoadingStationEndPose(); + + var phase1 = new CubicHermiteSplineParameters(); + var phase2 = new CubicHermiteSplineParameters(); + + switch (startingFace) { + case FAR_LEFT: + case CLOSE_LEFT: + case CLOSE: + case CLOSE_RIGHT: + // All of these work with the basic "get away from face then go straight to station" + phase1 = new CubicHermiteSplineParameters( + currentPose.getTranslation(), + loadingStationEndPose.getTranslation(), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.getReefFacePose(startingFace)).getRotation() + .plus(new Rotation2d(Math.PI))), + new Translation2d(5, loadingStationEndPose.getRotation() + .plus(new Rotation2d(Math.PI))) + ); + splines.add(new CubicHermiteSpline(phase1)); + break; + case FAR: + // This one needs a little more work to get around the reef + phase1 = new CubicHermiteSplineParameters( + currentPose.getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustLeftOfReef), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(45))), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180))) + ); + phase2 = new CubicHermiteSplineParameters( + phase1.endPoint(), + loadingStationEndPose.getTranslation(), + phase1.endControlVector(), + new Translation2d(5, loadingStationEndPose.getRotation() + .plus(new Rotation2d(Math.PI))) + ); + + splines.add(new CubicHermiteSpline(phase1)); + splines.add(new CubicHermiteSpline(phase2)); + break; + case FAR_RIGHT: + phase1 = new CubicHermiteSplineParameters( + currentPose.getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustAboveReef), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(-60))), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(90))) + ); + phase2 = new CubicHermiteSplineParameters( + phase1.endPoint(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustLeftOfReef), + phase1.endControlVector(), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180))) + ); + var phase3 = new CubicHermiteSplineParameters( + phase2.endPoint(), + loadingStationEndPose.getTranslation(), + phase2.endControlVector(), + new Translation2d(5, loadingStationEndPose.getRotation() + .plus(new Rotation2d(Math.PI))) + ); + + splines.add(new CubicHermiteSpline(phase1)); + splines.add(new CubicHermiteSpline(phase2)); + splines.add(new CubicHermiteSpline(phase3)); + break; + default: + } + + + if (endingStation == Landmarks.CoralStation.RIGHT) { + splines = CubicHermiteSpline.mirrorSplineLeftToRight(splines); + } + + return splines; + } + + public ArrayList getRouteFromLoadingStationToReef( + Landmarks.CoralStation startingStation, + Landmarks.ReefFace endingFace, Landmarks.Branch endingBranch) { + + var currentPose = pose.getCurrentPose2d(); + var startingControlVector = Landmarks.BlueLeftCoralStationMid.getRotation(); + + if (startingStation == Landmarks.CoralStation.RIGHT) { + endingFace = Landmarks.mirrorReefFaceLeftToRight(endingFace); + if (endingBranch == Landmarks.Branch.A) { + endingBranch = Landmarks.Branch.B; + } else { + endingBranch = Landmarks.Branch.A; + } + //startingControlVector = startingControlVector.times(-1); + // Pretend the robot is on the other side as well. + currentPose = new Pose2d( + new Translation2d(currentPose.getX(), PoseSubsystem.mirrorYCoordinateAcrossMidfield(currentPose.getY())), + currentPose.getRotation().times(-1)); + } + + var splines = new ArrayList(); + var reefEndPose = reefCoordinateGenerator.getPoseRelativeToReefFaceAndBranch( + DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue), + endingFace, + endingBranch, + Meters.of(1.25), + Meters.zero() + ); + + var phase1 = new CubicHermiteSplineParameters(); + var phase2 = new CubicHermiteSplineParameters(); + + switch (endingFace) { + case FAR_LEFT: + case CLOSE_LEFT: + case CLOSE: + case CLOSE_RIGHT: + phase1 = new CubicHermiteSplineParameters( + currentPose.getTranslation(), + reefEndPose.getTranslation(), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(startingControlVector)), + new Translation2d(5, reefEndPose.getRotation()) + ); + splines.add(new CubicHermiteSpline(phase1)); + break; + case FAR: + phase1 = new CubicHermiteSplineParameters( + currentPose.getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustLeftOfReef), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(startingControlVector)), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(0))) + ); + phase2 = new CubicHermiteSplineParameters( + phase1.endPoint(), + reefEndPose.getTranslation(), + phase1.endControlVector(), + new Translation2d(5, reefEndPose.getRotation()) + ); + splines.add(new CubicHermiteSpline(phase1)); + splines.add(new CubicHermiteSpline(phase2)); + break; + case FAR_RIGHT: + phase1 = new CubicHermiteSplineParameters( + currentPose.getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustLeftOfReef), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(startingControlVector)), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(0))) + ); + phase2 = new CubicHermiteSplineParameters( + phase1.endPoint(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustAboveReef), + phase1.endControlVector(), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(-90))) + ); + var phase3 = new CubicHermiteSplineParameters( + phase2.endPoint(), + reefEndPose.getTranslation(), + phase2.endControlVector(), + new Translation2d(5, reefEndPose.getRotation()) + ); + splines.add(new CubicHermiteSpline(phase1)); + splines.add(new CubicHermiteSpline(phase2)); + splines.add(new CubicHermiteSpline(phase3)); + break; + default: + break; + } + + if (startingStation == Landmarks.CoralStation.RIGHT) { + splines = CubicHermiteSpline.mirrorSplineLeftToRight(splines); + } + + return splines; + } + + private Pose2d getLeftLoadingStationEndPose() { + var coralStationPose = PoseSubsystem.convertBlueToRedIfNeeded( + Landmarks.getCoralStationSectionPose(Landmarks.CoralStation.LEFT, Landmarks.CoralStationSection.MID)); + // From the pose, project a point in front of it + var vectorToInterstitialPoint = new Translation2d(0.5, coralStationPose.getRotation()); + return new Pose2d( + coralStationPose.getTranslation().plus(vectorToInterstitialPoint), + coralStationPose.getRotation()); + } +} diff --git a/src/main/java/competition/motion/CubicHermiteSpline.java b/src/main/java/competition/motion/CubicHermiteSpline.java new file mode 100644 index 00000000..1cf78985 --- /dev/null +++ b/src/main/java/competition/motion/CubicHermiteSpline.java @@ -0,0 +1,335 @@ +package competition.motion; + + import competition.subsystems.pose.PoseSubsystem; + import edu.wpi.first.math.geometry.Translation2d; + + import java.util.ArrayList; + import java.util.List; + + import static edu.wpi.first.units.Units.Meters; + +/** + * Represents a Cubic Hermite spline with methods to set points, control vectors, + * and estimate arc length using Gauss-Legendre quadrature. + */ + public class CubicHermiteSpline { + // Points + private Translation2d startPoint; + private Translation2d endPoint; + + // Control vectors + private Translation2d startControlVector; + private Translation2d endControlVector; + + // Lookup table for arc length parameterization + private boolean arcLengthTableInitialized = false; + private double[] arcLengthTable; + private int lookupTableSize = 50; // Balance between accuracy and performance + + /** + * Creates a new Cubic Hermite spline with all points and vectors initialized to zero. + */ + public CubicHermiteSpline() { + startPoint = new Translation2d(0.0, 0.0); + endPoint = new Translation2d(0.0, 0.0); + startControlVector = new Translation2d(0.0, 0.0); + endControlVector = new Translation2d(0.0, 0.0); + } + + public CubicHermiteSpline(CubicHermiteSplineParameters params) { + setParameters(params); + } + + /** + * Sets the starting point of the spline. + */ + public void setStartPoint(Translation2d point) { + this.startPoint = point; + } + + /** + * Sets the end point of the spline. + */ + public void setEndPoint(Translation2d point) { + this.endPoint = point; + } + + /** + * Sets the starting control vector using magnitude and direction. + */ + public void setStartControlVector(double magnitude, double directionRadians) { + double x = magnitude * Math.cos(directionRadians); + double y = magnitude * Math.sin(directionRadians); + this.startControlVector = new Translation2d(x, y); + } + + /** + * Sets the starting control vector using a Translation2d. + */ + public void setStartControlVector(Translation2d vector) { + this.startControlVector = vector; + } + + /** + * Sets the end control vector using magnitude and direction. + */ + public void setEndControlVector(double magnitude, double directionRadians) { + double x = magnitude * Math.cos(directionRadians); + double y = magnitude * Math.sin(directionRadians); + this.endControlVector = new Translation2d(x, y); + } + + /** + * Sets the end control vector using a Translation2d. + */ + public void setEndControlVector(Translation2d vector) { + this.endControlVector = vector; + } + + public void setParameters(CubicHermiteSplineParameters params) { + setStartPoint(params.startPoint()); + setEndPoint(params.endPoint()); + setStartControlVector(params.startControlVector()); + setEndControlVector(params.endControlVector()); + } + + /** + * Evaluates the spline at parameter t. + */ + public Translation2d evaluate(double t) { + // Hermite basis functions + double h00 = 2*t*t*t - 3*t*t + 1; + double h10 = t*t*t - 2*t*t + t; + double h01 = -2*t*t*t + 3*t*t; + double h11 = t*t*t - t*t; + + double x = h00 * startPoint.getX() + h10 * startControlVector.getX() + + h01 * endPoint.getX() + h11 * endControlVector.getX(); + double y = h00 * startPoint.getY() + h10 * startControlVector.getY() + + h01 * endPoint.getY() + h11 * endControlVector.getY(); + + return new Translation2d(x, y); + } + + /** + * Evaluates the x-coordinate of the spline at parameter t. + */ + public double evaluateX(double t) { + // Hermite basis functions + double h00 = 2*t*t*t - 3*t*t + 1; + double h10 = t*t*t - 2*t*t + t; + double h01 = -2*t*t*t + 3*t*t; + double h11 = t*t*t - t*t; + + return h00 * startPoint.getX() + h10 * startControlVector.getX() + + h01 * endPoint.getX() + h11 * endControlVector.getX(); + } + + /** + * Evaluates the y-coordinate of the spline at parameter t. + */ + public double evaluateY(double t) { + // Hermite basis functions + double h00 = 2*t*t*t - 3*t*t + 1; + double h10 = t*t*t - 2*t*t + t; + double h01 = -2*t*t*t + 3*t*t; + double h11 = t*t*t - t*t; + + return h00 * startPoint.getY() + h10 * startControlVector.getY() + + h01 * endPoint.getY() + h11 * endControlVector.getY(); + } + + /** + * Computes the derivative at parameter t. + */ + public Translation2d derivative(double t) { + double dh00 = 6*t*t - 6*t; + double dh10 = 3*t*t - 4*t + 1; + double dh01 = -6*t*t + 6*t; + double dh11 = 3*t*t - 2*t; + + double dx = dh00 * startPoint.getX() + dh10 * startControlVector.getX() + + dh01 * endPoint.getX() + dh11 * endControlVector.getX(); + double dy = dh00 * startPoint.getY() + dh10 * startControlVector.getY() + + dh01 * endPoint.getY() + dh11 * endControlVector.getY(); + + return new Translation2d(dx, dy); + } + + /** + * Computes the derivative of x with respect to t. + */ + public double derivativeX(double t) { + double dh00 = 6*t*t - 6*t; + double dh10 = 3*t*t - 4*t + 1; + double dh01 = -6*t*t + 6*t; + double dh11 = 3*t*t - 2*t; + + return dh00 * startPoint.getX() + dh10 * startControlVector.getX() + + dh01 * endPoint.getX() + dh11 * endControlVector.getX(); + } + + /** + * Computes the derivative of y with respect to t. + */ + public double derivativeY(double t) { + double dh00 = 6*t*t - 6*t; + double dh10 = 3*t*t - 4*t + 1; + double dh01 = -6*t*t + 6*t; + double dh11 = 3*t*t - 2*t; + + return dh00 * startPoint.getY() + dh10 * startControlVector.getY() + + dh01 * endPoint.getY() + dh11 * endControlVector.getY(); + } + + /** + * Estimates the arc length of the spline using 5-point Gauss-Legendre quadrature. + */ + public double estimateArcLength() { + // 5-point Gauss-Legendre quadrature points and weights + double[] points = { + -0.9061798459, + -0.5384693101, + 0.0, + 0.5384693101, + 0.9061798459 + }; + + double[] weights = { + 0.2369268850, + 0.4786286705, + 0.5688888889, + 0.4786286705, + 0.2369268850 + }; + + double sum = 0.0; + + // Scale from [-1, 1] to [0, 1] + for (int i = 0; i < points.length; i++) { + double t = (points[i] + 1) / 2.0; + double dx = derivativeX(t); + double dy = derivativeY(t); + double integrand = Math.sqrt(dx * dx + dy * dy); + sum += weights[i] * integrand; + } + + // Scale factor for interval change + return sum * 0.5; + } + + /** + * Returns the parameter t corresponding to a given distance along the spline. + * Uses a precomputed lookup table for efficiency. + * + * @param distance The distance along the spline from the start point + * @return The parameter t (between 0 and 1) corresponding to that distance + */ + public double getParameterFromDistance(double distance) { + // Handle edge cases + if (distance <= 0) { + return 0.0; + } + + // Initialize table if needed + if (!arcLengthTableInitialized) { + initializeArcLengthTable(); + } + + double totalLength = arcLengthTable[lookupTableSize - 1]; + if (distance >= totalLength) { + return 1.0; + } + + // Binary search to find the segment containing our target distance + int low = 0; + int high = lookupTableSize - 1; + + while (low < high - 1) { + int mid = (low + high) / 2; + if (arcLengthTable[mid] < distance) { + low = mid; + } else { + high = mid; + } + } + + // Linear interpolation between table entries + double t0 = (double)low / (lookupTableSize - 1); + double t1 = (double)high / (lookupTableSize - 1); + double s0 = arcLengthTable[low]; + double s1 = arcLengthTable[high]; + + return t0 + (t1 - t0) * (distance - s0) / (s1 - s0); + } + + /** + * Initialize the arc length lookup table using Gauss-Legendre quadrature + */ + public void initializeArcLengthTable() { + arcLengthTable = new double[lookupTableSize]; + arcLengthTable[0] = 0; + + // 3-point Gauss-Legendre quadrature for efficiency + double[] points = {-0.7745966692, 0.0, 0.7745966692}; + double[] weights = {0.5555555556, 0.8888888889, 0.5555555556}; + + // For each segment in our lookup table + for (int i = 1; i < lookupTableSize; i++) { + double t = (double)i / (lookupTableSize - 1); + double tPrev = (double)(i-1) / (lookupTableSize - 1); + double segmentLength = 0; + + // Apply Gauss-Legendre for this segment + for (int j = 0; j < points.length; j++) { + // Map from [-1, 1] to [tPrev, t] + double tau = ((t - tPrev) * points[j] + t + tPrev) / 2; + + double dx = derivativeX(tau); + double dy = derivativeY(tau); + double speed = Math.sqrt(dx * dx + dy * dy); + + segmentLength += weights[j] * speed; + } + + // Scale for the interval and add to total + segmentLength *= (t - tPrev) / 2; + arcLengthTable[i] = arcLengthTable[i-1] + segmentLength; + } + + arcLengthTableInitialized = true; + } + + // Getters + public Translation2d getStartPoint() { return startPoint; } + public Translation2d getEndPoint() { return endPoint; } + public Translation2d getStartControlVector() { return startControlVector; } + public Translation2d getEndControlVector() { return endControlVector; } + + // Compatibility methods for legacy code + public double getStartX() { return startPoint.getX(); } + public double getStartY() { return startPoint.getY(); } + public double getEndX() { return endPoint.getX(); } + public double getEndY() { return endPoint.getY(); } + public double getStartControlVectorX() { return startControlVector.getX(); } + public double getStartControlVectorY() { return startControlVector.getY(); } + public double getEndControlVectorX() { return endControlVector.getX(); } + public double getEndControlVectorY() { return endControlVector.getY(); } + + public static CubicHermiteSpline mirrorSplineLeftToRight(CubicHermiteSpline spline) { + // This will mirror the spline across the Y midpoint of the field, leaving the X values unchanged. + // The control vectors will also need to mirror in the same way. + + return new CubicHermiteSpline(new CubicHermiteSplineParameters( + new Translation2d(spline.getStartX(),PoseSubsystem.mirrorYCoordinateAcrossMidfield(spline.getStartY())), + new Translation2d(spline.getEndX(),PoseSubsystem.mirrorYCoordinateAcrossMidfield(spline.getEndY())), + new Translation2d(spline.getStartControlVectorX(), -spline.getStartControlVectorY()), + new Translation2d(spline.getEndControlVectorX(), -spline.getEndControlVectorY()) + )); + } + + public static ArrayList mirrorSplineLeftToRight(ArrayList splines) { + splines.replaceAll(CubicHermiteSpline::mirrorSplineLeftToRight); + return splines; + } + } \ No newline at end of file diff --git a/src/main/java/competition/motion/CubicHermiteSplineParameters.java b/src/main/java/competition/motion/CubicHermiteSplineParameters.java new file mode 100644 index 00000000..85458212 --- /dev/null +++ b/src/main/java/competition/motion/CubicHermiteSplineParameters.java @@ -0,0 +1,9 @@ +package competition.motion; + +import edu.wpi.first.math.geometry.Translation2d; + +public record CubicHermiteSplineParameters(Translation2d startPoint, Translation2d endPoint, Translation2d startControlVector, Translation2d endControlVector) { + public CubicHermiteSplineParameters() { + this(new Translation2d(), new Translation2d(), new Translation2d(), new Translation2d()); + } +} diff --git a/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java b/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java new file mode 100644 index 00000000..a6016d92 --- /dev/null +++ b/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java @@ -0,0 +1,249 @@ +package competition.motion; + +import edu.wpi.first.math.geometry.Translation2d; + +import javax.swing.JButton; +import javax.swing.JFrame; +import javax.swing.JPanel; +import javax.swing.SwingUtilities; +import javax.swing.Timer; +import java.awt.BasicStroke; +import java.awt.BorderLayout; +import java.awt.Color; +import java.awt.Graphics; +import java.awt.Graphics2D; +import java.awt.RenderingHints; +import java.awt.event.ActionEvent; +import java.awt.event.ActionListener; +import java.awt.geom.AffineTransform; + + +public class CubicHermiteSplineVisualizer extends JFrame { + + private static final double CANVAS_SIZE = 800; + private static final double SCALE = 100; + private static final double OFFSET = CANVAS_SIZE / 2; + + private CubicHermiteSpline spline; + private double currentT = 0.0; + private Timer animationTimer; + private boolean isAnimating = false; + private static final double ANIMATION_SPEED = 0.005; // Increment per timer tick + + private DrawPanel drawPanel; + private JButton startButton; + private JButton stopButton; + private JButton restartButton; + + public CubicHermiteSplineVisualizer() { + setTitle("Cubic Hermite Spline Visualizer"); + setSize((int) CANVAS_SIZE, (int) (CANVAS_SIZE + 50)); // Extra space for buttons + setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + + // Create a sample spline + spline = new CubicHermiteSpline(); + spline.setStartPoint(new Translation2d( -3, 1)); + spline.setEndPoint(new Translation2d(3, -3)); + spline.setStartControlVector(6, 0); + spline.setEndControlVector(6, +Math.PI/2); + + // Create drawing panel + drawPanel = new DrawPanel(); + add(drawPanel, BorderLayout.CENTER); + + // Create control panel + JPanel controlPanel = new JPanel(); + startButton = new JButton("Start"); + stopButton = new JButton("Stop"); + restartButton = new JButton("Restart"); + + controlPanel.add(startButton); + controlPanel.add(stopButton); + controlPanel.add(restartButton); + add(controlPanel, BorderLayout.SOUTH); + + // Set up the animation timer + animationTimer = new Timer(16, new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + if (currentT < 1.0) { + currentT += ANIMATION_SPEED; + if (currentT > 1.0) { + currentT = 1.0; + } + drawPanel.repaint(); + } else { + animationTimer.stop(); + isAnimating = false; + } + } + }); + + // Configure button actions + startButton.addActionListener(e -> { + if (!isAnimating) { + isAnimating = true; + animationTimer.start(); + } + }); + + stopButton.addActionListener(e -> { + if (isAnimating) { + animationTimer.stop(); + isAnimating = false; + } + }); + + restartButton.addActionListener(e -> { + currentT = 0.0; + drawPanel.repaint(); + if (isAnimating) { + animationTimer.stop(); + animationTimer.start(); + } + }); + } + + private class DrawPanel extends JPanel { + @Override + protected void paintComponent(Graphics g) { + super.paintComponent(g); + Graphics2D g2d = (Graphics2D) g; + g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); + draw(g2d); + } + + private void draw(Graphics2D g2d) { + // Draw coordinate axes + g2d.setColor(Color.LIGHT_GRAY); + g2d.drawLine(0, (int) OFFSET, (int) CANVAS_SIZE, (int) OFFSET); // X-axis + g2d.drawLine((int) OFFSET, 0, (int) OFFSET, (int) CANVAS_SIZE); // Y-axis + + // Draw the spline curve + g2d.setColor(Color.BLUE); + g2d.setStroke(new BasicStroke(2.0f)); + + double prevX = spline.evaluateX(0); + double prevY = spline.evaluateY(0); + + for (double t = 0.01; t <= 1.0; t += 0.01) { + double x = spline.evaluateX(t); + double y = spline.evaluateY(t); + + g2d.drawLine( + (int) (OFFSET + prevX * SCALE), + (int) (OFFSET - prevY * SCALE), + (int) (OFFSET + x * SCALE), + (int) (OFFSET - y * SCALE) + ); + + prevX = x; + prevY = y; + } + + // Draw start and end points + int startPointSize = 8; + g2d.setColor(Color.GREEN); + g2d.fillOval( + (int) (OFFSET + spline.getStartX() * SCALE - startPointSize/2), + (int) (OFFSET - spline.getStartY() * SCALE - startPointSize/2), + startPointSize, startPointSize + ); + + g2d.setColor(Color.MAGENTA); + g2d.fillOval( + (int) (OFFSET + spline.getEndX() * SCALE - startPointSize/2), + (int) (OFFSET - spline.getEndY() * SCALE - startPointSize/2), + startPointSize, startPointSize + ); + + // Draw control vectors as arrows + drawArrow(g2d, + spline.getStartX(), spline.getStartY(), + spline.getStartX() + spline.getStartControlVectorX(), + spline.getStartY() + spline.getStartControlVectorY(), + Color.ORANGE + ); + + drawArrow(g2d, + spline.getEndX(), spline.getEndY(), + spline.getEndX() + spline.getEndControlVectorX(), + spline.getEndY() + spline.getEndControlVectorY(), + Color.CYAN + ); + + // Draw current position if animating or paused + if (currentT > 0) { + double currentX = spline.evaluateX(currentT); + double currentY = spline.evaluateY(currentT); + + g2d.setColor(Color.RED); + int currentPointSize = 12; + g2d.fillOval( + (int) (OFFSET + currentX * SCALE - currentPointSize/2), + (int) (OFFSET - currentY * SCALE - currentPointSize/2), + currentPointSize, currentPointSize + ); + } + + // Draw time information + g2d.setColor(Color.BLACK); + g2d.drawString(String.format("t = %.2f", currentT), 20, 20); + g2d.drawString(String.format("Arc Length = %.2f", spline.estimateArcLength()), 20, 40); + + // Calculate and display velocity + if (currentT > 0) { + // Get current derivatives (velocity components) + double dx = spline.derivativeX(currentT); + double dy = spline.derivativeY(currentT); + + // Calculate velocity magnitude + double velocity = Math.sqrt(dx * dx + dy * dy); + + // Display velocity + g2d.drawString(String.format("Velocity = %.2f", velocity), 20, 60); + } + } + + private void drawArrow(Graphics2D g2d, double x1, double y1, double x2, double y2, Color color) { + g2d.setColor(color); + + // Convert to screen coordinates + double sx1 = OFFSET + x1 * SCALE; + double sy1 = OFFSET - y1 * SCALE; + double sx2 = OFFSET + x2 * SCALE; + double sy2 = OFFSET - y2 * SCALE; + + // Draw main line + g2d.drawLine((int)sx1, (int)sy1, (int)sx2, (int)sy2); + + // Calculate arrowhead + double arrowSize = 10; + double dx = sx2 - sx1; + double dy = sy2 - sy1; + double angle = Math.atan2(dy, dx); + + AffineTransform tx = new AffineTransform(); + tx.setToIdentity(); + tx.translate(sx2, sy2); + tx.rotate(angle); + + // Create arrowhead shape + int[] arrowX = {0, -10, -10}; + int[] arrowY = {0, -5, 5}; + + g2d.setTransform(tx); + g2d.fillPolygon(arrowX, arrowY, 3); + + // Reset transform + g2d.setTransform(new AffineTransform()); + } + } + + public static void main(String[] args) { + SwingUtilities.invokeLater(() -> { + CubicHermiteSplineVisualizer visualizer = new CubicHermiteSplineVisualizer(); + visualizer.setVisible(true); + }); + } +} \ No newline at end of file diff --git a/src/main/java/competition/motion/HermiteTrajectory.java b/src/main/java/competition/motion/HermiteTrajectory.java new file mode 100644 index 00000000..e7f56a8e --- /dev/null +++ b/src/main/java/competition/motion/HermiteTrajectory.java @@ -0,0 +1,116 @@ +package competition.motion; + + import edu.wpi.first.math.geometry.Translation2d; + import edu.wpi.first.math.trajectory.TrapezoidProfile; + import xbot.common.controls.sensors.XTimer; + + import javax.inject.Inject; + import java.util.ArrayList; + import java.util.List; + + public class HermiteTrajectory { + + private List splines; + private TrapezoidProfile trapezoid; + private double startTimeInSeconds = 0; + private double elapsedSeconds = 0; + private double frozenOffsetSeconds = 0; + private final double maxGap = 0.75; + private TrapezoidProfile.State initialState; + private TrapezoidProfile.State finalState; + + // Store cumulative arc lengths for each spline + private double[] cumulativeArcLengths; + + @Inject + public HermiteTrajectory() { + trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(4.5, 6)); + splines = new ArrayList<>(); + } + + public void setSplines(List splines) { + this.splines = splines; + } + + public void setSpline(CubicHermiteSpline spline) { + this.splines = new ArrayList<>(); + this.splines.add(spline); + } + + public void initialize(double currentVelocityMetersPerSecond, double finalVelocityMetersPerSecond) { + // Initialize arc length tables for all splines + for (CubicHermiteSpline spline : splines) { + spline.initializeArcLengthTable(); + } + + // Calculate cumulative arc lengths + calculateCumulativeArcLengths(); + + // Total arc length is last value in cumulativeArcLengths + double arcLengthMeters = cumulativeArcLengths[splines.size() - 1]; + + startTimeInSeconds = XTimer.getFPGATimestamp(); + initialState = new TrapezoidProfile.State(0, currentVelocityMetersPerSecond); + finalState = new TrapezoidProfile.State(arcLengthMeters, finalVelocityMetersPerSecond); + frozenOffsetSeconds = 0; + elapsedSeconds = 0; + } + + private void calculateCumulativeArcLengths() { + cumulativeArcLengths = new double[splines.size()]; + double totalLength = 0; + + for (int i = 0; i < splines.size(); i++) { + double splineLength = splines.get(i).estimateArcLength(); + totalLength += splineLength; + cumulativeArcLengths[i] = totalLength; + } + } + + public HermiteTrajectoryAdvice advise(Translation2d currentPosition) { + double elapsedSecondsCandidate = XTimer.getFPGATimestamp() - startTimeInSeconds - frozenOffsetSeconds; + var candidateAdvice = getAdviceForTime(elapsedSecondsCandidate, false); + + if (Math.abs(candidateAdvice.position().getDistance(currentPosition)) > maxGap) { + candidateAdvice = getAdviceForTime(elapsedSeconds, true); + frozenOffsetSeconds = XTimer.getFPGATimestamp() - startTimeInSeconds - elapsedSeconds; + } else { + elapsedSeconds = elapsedSecondsCandidate; + } + + return candidateAdvice; + } + + private HermiteTrajectoryAdvice getAdviceForTime(double elapsedSeconds, boolean timeFrozen) { + var trapezoidAdvice = trapezoid.calculate(elapsedSeconds, initialState, finalState); + double totalDistance = trapezoidAdvice.position; + + // Find which spline contains this distance + int splineIndex = 0; + double previousCumulativeLength = 0; + + for (int i = 0; i < cumulativeArcLengths.length; i++) { + if (totalDistance <= cumulativeArcLengths[i]) { + splineIndex = i; + break; + } + previousCumulativeLength = cumulativeArcLengths[i]; + } + + // Calculate local distance within the selected spline + double localDistance = totalDistance - previousCumulativeLength; + + // Get the correct spline and evaluate it + CubicHermiteSpline currentSpline = splines.get(splineIndex); + double splineLength = (splineIndex == 0) ? cumulativeArcLengths[0] : + cumulativeArcLengths[splineIndex] - cumulativeArcLengths[splineIndex-1]; + double lerp = currentSpline.getParameterFromDistance(localDistance); + + var position = currentSpline.evaluate(lerp); + var direction = currentSpline.derivative(lerp); + var velocity = new Translation2d(trapezoidAdvice.velocity, direction.getAngle()); + + return new HermiteTrajectoryAdvice(position, velocity, timeFrozen, + finalState.position - totalDistance, trapezoidAdvice.position); + } + } \ No newline at end of file diff --git a/src/main/java/competition/motion/HermiteTrajectoryAdvice.java b/src/main/java/competition/motion/HermiteTrajectoryAdvice.java new file mode 100644 index 00000000..529233e7 --- /dev/null +++ b/src/main/java/competition/motion/HermiteTrajectoryAdvice.java @@ -0,0 +1,6 @@ +package competition.motion; + +import edu.wpi.first.math.geometry.Translation2d; + +public record HermiteTrajectoryAdvice(Translation2d position, Translation2d velocity, boolean timeFrozen, double timeRemaining, double distanceTravelled) { +} diff --git a/src/main/java/competition/motion/TrapezoidProfileAdvice.java b/src/main/java/competition/motion/TrapezoidProfileAdvice.java new file mode 100644 index 00000000..8ed8649d --- /dev/null +++ b/src/main/java/competition/motion/TrapezoidProfileAdvice.java @@ -0,0 +1,4 @@ +package competition.motion; + +public record TrapezoidProfileAdvice(double position, double velocity) { +} diff --git a/src/main/java/competition/motion/TrapezoidProfileManager.java b/src/main/java/competition/motion/TrapezoidProfileManager.java index e04727be..e69d78f4 100644 --- a/src/main/java/competition/motion/TrapezoidProfileManager.java +++ b/src/main/java/competition/motion/TrapezoidProfileManager.java @@ -130,5 +130,12 @@ public double getRecommendedPositionForTime() { previousSetpoint = setpoint.position; return setpoint.position; } + + public TrapezoidProfileAdvice getRecommendedPositionAndVelocityForTime() { + var setpoint = profile.calculate(XTimer.getFPGATimestampTime().minus(profileStartTime).in(Seconds), initialState, goalState); + this.previousSetpointTime.mut_replace(XTimer.getFPGATimestampTime()); + previousSetpoint = setpoint.position; + return new TrapezoidProfileAdvice(setpoint.position, setpoint.velocity); + } } diff --git a/src/main/java/competition/operator_interface/NeoTrellisSubsystem.java b/src/main/java/competition/operator_interface/NeoTrellisSubsystem.java index 27b2d268..050efcc8 100644 --- a/src/main/java/competition/operator_interface/NeoTrellisSubsystem.java +++ b/src/main/java/competition/operator_interface/NeoTrellisSubsystem.java @@ -51,6 +51,31 @@ public class NeoTrellisSubsystem extends BaseSubsystem { final Latch comboDetectedLatch; + // Reef face button indices + public static final int REEF_CLOSE_A = 27; + public static final int REEF_CLOSE_B = 28; + public static final int REEF_CLOSE_RIGHT_A = 29; + public static final int REEF_CLOSE_RIGHT_B = 22; + public static final int REEF_FAR_RIGHT_A = 14; + public static final int REEF_FAR_RIGHT_B = 5; + public static final int REEF_FAR_A = 4; + public static final int REEF_FAR_B = 3; + public static final int REEF_FAR_LEFT_A = 2; + public static final int REEF_FAR_LEFT_B = 9; + public static final int REEF_CLOSE_LEFT_A = 17; + public static final int REEF_CLOSE_LEFT_B = 26; + + // Coral level button indices + public static final int CORAL_LEVEL_ONE = 32; + public static final int CORAL_LEVEL_TWO = 24; + public static final int CORAL_LEVEL_THREE = 16; + public static final int CORAL_LEVEL_FOUR = 8; + + // Action button indices + public static final int REMOVE_ALGAE_BUTTON = 19; + public static final int PROCESS_ALGAE_BUTTON = 20; + public static final int CLEAR_QUEUE_BUTTON = 11; + @Inject public NeoTrellisSubsystem(OperatorInterface oi, ScoringQueue scoringQueue) { this.oi = oi; @@ -64,9 +89,9 @@ public NeoTrellisSubsystem(OperatorInterface oi, ScoringQueue scoringQueue) { levelsToButtonIndices = new HashMap<>(); initializeLocationsAndLevels(); - removeAlgaeButton = neoTrellis.getifAvailable(19); - processAlgaeButton = neoTrellis.getifAvailable(20); - resetQueueButton = neoTrellis.getifAvailable(11); + removeAlgaeButton = neoTrellis.getifAvailable(REMOVE_ALGAE_BUTTON); + processAlgaeButton = neoTrellis.getifAvailable(PROCESS_ALGAE_BUTTON); + resetQueueButton = neoTrellis.getifAvailable(CLEAR_QUEUE_BUTTON); // Resetting doesn't need any other button to be pressed, so we set it up as a typical // "press this button and get this command" binding. @@ -79,17 +104,17 @@ public NeoTrellisSubsystem(OperatorInterface oi, ScoringQueue scoringQueue) { } protected void initializeLocationsAndLevels() { - initializeLocationPair(Landmarks.ReefFace.CLOSE, 27, 28); - initializeLocationPair(Landmarks.ReefFace.CLOSE_RIGHT, 29, 22); - initializeLocationPair(Landmarks.ReefFace.FAR_RIGHT, 14, 5); - initializeLocationPair(Landmarks.ReefFace.FAR, 4, 3); - initializeLocationPair(Landmarks.ReefFace.FAR_LEFT, 2, 9); - initializeLocationPair(Landmarks.ReefFace.CLOSE_LEFT,17, 26); - - initializeLevel(Landmarks.CoralLevel.ONE, 32); - initializeLevel(Landmarks.CoralLevel.TWO, 24); - initializeLevel(Landmarks.CoralLevel.THREE, 16); - initializeLevel(Landmarks.CoralLevel.FOUR, 8); + initializeLocationPair(Landmarks.ReefFace.CLOSE, REEF_CLOSE_A, REEF_CLOSE_B); + initializeLocationPair(Landmarks.ReefFace.CLOSE_RIGHT, REEF_CLOSE_RIGHT_A, REEF_CLOSE_RIGHT_B); + initializeLocationPair(Landmarks.ReefFace.FAR_RIGHT, REEF_FAR_RIGHT_A, REEF_FAR_RIGHT_B); + initializeLocationPair(Landmarks.ReefFace.FAR, REEF_FAR_A, REEF_FAR_B); + initializeLocationPair(Landmarks.ReefFace.FAR_LEFT, REEF_FAR_LEFT_A, REEF_FAR_LEFT_B); + initializeLocationPair(Landmarks.ReefFace.CLOSE_LEFT,REEF_CLOSE_LEFT_A, REEF_CLOSE_LEFT_B); + + initializeLevel(Landmarks.CoralLevel.ONE, CORAL_LEVEL_ONE); + initializeLevel(Landmarks.CoralLevel.TWO, CORAL_LEVEL_TWO); + initializeLevel(Landmarks.CoralLevel.THREE, CORAL_LEVEL_THREE); + initializeLevel(Landmarks.CoralLevel.FOUR, CORAL_LEVEL_FOUR); } protected void initializeLevel(Landmarks.CoralLevel level, int buttonNumber) { @@ -197,15 +222,15 @@ public int getNeoTrellisButtonIndex(Landmarks.CoralLevel level) { } public int getNeoTrellisButtonAlgaeRemoval() { - return 19; + return REMOVE_ALGAE_BUTTON; } public int getNeoTrellisButtonAlgaeProcessing() { - return 20; + return PROCESS_ALGAE_BUTTON; } public int getNeoTrellisButtonClearQueue() { - return 11; + return CLEAR_QUEUE_BUTTON; } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 2fcd7bd5..f85f1746 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -1,8 +1,10 @@ package competition.operator_interface; +import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import competition.auto_programs.FromCageScoreOneCoralAutoFactory; import competition.auto_programs.FromLeftCageScoreLeftFacesLevelFours; import competition.auto_programs.FromRightCageScoreRightFacesLevelFours; +import competition.commandgroups.DriveToFaceAndScoreCommandGroupFactory; import competition.commandgroups.HeadingAssistedDriveAndScoreCommandGroup; import competition.commandgroups.PrepAlgaeSystemCommandGroupFactory; import competition.commandgroups.PrepCoralSystemCommandGroupFactory; @@ -26,12 +28,14 @@ import competition.subsystems.drive.commands.AlignToSpecificHumanLoadingStationCommand; import competition.subsystems.drive.commands.CalibrateDriveCommand; import competition.subsystems.drive.commands.DebugSwerveModuleCommand; +import competition.subsystems.drive.commands.DriveHermiteSplineCommand; import competition.subsystems.drive.commands.DriveToClosestReefSectionWithVisionCommand; import competition.subsystems.drive.commands.DriveToCoralStationInterstitialCommand; import competition.subsystems.drive.commands.DriveToCoralStationWithVisionCommand; import competition.subsystems.drive.commands.DriveToLocationWithPID; import competition.subsystems.drive.commands.DriveToNearestReefFaceWithPID; import competition.subsystems.drive.commands.RotateToHeadingWithHeadingModule; +import competition.subsystems.drive.commands.ShoveCoralStationCommand; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.elevator.ElevatorSubsystem; import competition.subsystems.elevator.commands.ForceElevatorCalibratedCommand; @@ -44,9 +48,12 @@ import competition.subsystems.pose.commands.ResetPoseCommand; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import xbot.common.command.SmartDashboardCommandPutter; import xbot.common.controls.sensors.XXboxController; +import xbot.common.subsystems.autonomous.AutonomousCommandSelector; import xbot.common.subsystems.autonomous.SetAutonomousCommand; import xbot.common.subsystems.drive.SwervePointKinematics; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; @@ -83,6 +90,8 @@ public void setupDriverCommands( ChangeActiveSwerveModuleCommand changeActiveModule, SwerveDriveWithJoysticksCommand typicalSwerveDrive, DriveToNearestReefFaceWithPID driveToNearestReefFaceWithPID, + AlignToNearestCoralStationCommand alignToNearestCoralStationCommand, + DriveHermiteSplineCommand hermite, DriveSubsystem drive, PoseSubsystem pose) { resetHeading.setHeadingToApply(0); operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.Start).onTrue(resetHeading); @@ -106,6 +115,16 @@ public void setupDriverCommands( .onFalse(clearPointAtHeading); operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.X).whileTrue(driveToNearestReefFaceWithPID); + operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); + operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); + operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(typicalSwerveDrive); + + //operatorInterface.driverGamepad.getPovIfAvailable(270).whileTrue(hermite); + + // (BLUE ALLIANCE) Below are different routes to test the SwerveSimpleTrajectoryCommand + // I don't think createPotentiallyFilppedXbotSwervePoint works under OperatorCommandMap + SwervePointKinematics kinematicValuesForTesting = new SwervePointKinematics(1, 0, 0, 5); + // operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); // operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); // operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(typicalSwerveDrive); @@ -307,6 +326,67 @@ public void setupAutonomousCommands(OperatorInterface oi, setFromRightCageScoreRightFacesLevelFours.includeOnSmartDashboard("From Right Score Right Face Level Fours auto"); } + @Inject + public void setupDirectRouteCommands( + OperatorInterface oi, + Provider splineDriveProvider, + Provider alignToReefWithAprilTagProvider, + Provider shoveStationProvider, + AutonomousCommandSelector selector, + SmartDashboardCommandPutter commandPutter, + Provider intakeCoralCommandProvider, + DriveToFaceAndScoreCommandGroupFactory driveToFaceAndScoreFactory, + PrepCoralSystemCommandGroupFactory prepCoralSystemCommandGroupFactory) { + + var collectLeft = splineDriveProvider.get(); + collectLeft.configureForStation(Landmarks.CoralStation.LEFT); + var shoveLeft = shoveStationProvider.get(); + shoveLeft.setShoveAngle(Landmarks.CoralStation.LEFT); + var intakeCoralLeft = intakeCoralCommandProvider.get(); + var lowerSuperstructureForCollection = prepCoralSystemCommandGroupFactory.create(() -> Landmarks.CoralLevel.COLLECTING); + var collectLeftAndShove = collectLeft.alongWith(lowerSuperstructureForCollection).andThen(shoveLeft.alongWith(intakeCoralLeft)); + SmartDashboard.putData("Collect Left", collectLeftAndShove); + + var collectRight = splineDriveProvider.get(); + collectRight.configureForStation(Landmarks.CoralStation.RIGHT); + collectRight.includeOnSmartDashboard("Collect Right"); + + var scoreClose = splineDriveProvider.get(); + scoreClose.configureForReef(Landmarks.ReefFace.CLOSE, Landmarks.Branch.A); + scoreClose.includeOnSmartDashboard("Score Close"); + + var scoreCloseLeft = splineDriveProvider.get(); + scoreCloseLeft.configureForReef(Landmarks.ReefFace.CLOSE_LEFT, Landmarks.Branch.A); + scoreCloseLeft.includeOnSmartDashboard("Score Close Left"); + + var scoreCloseRight = splineDriveProvider.get(); + scoreCloseRight.configureForReef(Landmarks.ReefFace.CLOSE_RIGHT, Landmarks.Branch.A); + scoreCloseRight.includeOnSmartDashboard("Score Close Right"); + + var scoreFar = splineDriveProvider.get(); + scoreFar.configureForReef(Landmarks.ReefFace.FAR, Landmarks.Branch.A); + scoreFar.includeOnSmartDashboard("Score Far"); + + var scoreFarLeft = splineDriveProvider.get(); + scoreFarLeft.configureForReef(Landmarks.ReefFace.FAR_LEFT, Landmarks.Branch.A); + scoreFarLeft.includeOnSmartDashboard("Score Far Left"); + + var scoreFarRight = splineDriveProvider.get(); + scoreFarRight.configureForReef(Landmarks.ReefFace.FAR_RIGHT, Landmarks.Branch.A); + scoreFarRight.includeOnSmartDashboard("Score Far Right"); + + var actuallyScoreClose = new BaseAutonomousSequentialCommandGroup(selector); + var splineToClose = splineDriveProvider.get(); + splineToClose.configureForReef(Landmarks.ReefFace.CLOSE, Landmarks.Branch.A); + + + var womboCombo = driveToFaceAndScoreFactory.create( + Landmarks.ReefFace.CLOSE, Landmarks.Branch.A, Landmarks.CoralLevel.FOUR, true); + SmartDashboard.putData("ActuallyScoreClose", womboCombo); + + oi.driverGamepad.getPovIfAvailable(270).whileTrue(womboCombo); + } + @Inject public void setupSimulatorCommands( ResetSimulatedPose resetPose diff --git a/src/main/java/competition/simulation/MapleSimulator.java b/src/main/java/competition/simulation/MapleSimulator.java index 5c913699..bbbf2604 100644 --- a/src/main/java/competition/simulation/MapleSimulator.java +++ b/src/main/java/competition/simulation/MapleSimulator.java @@ -240,7 +240,10 @@ protected void updateDriveSimulation() { // tell the pose subystem about where the robot has moved based on odometry pose.ingestSimulatedSwerveModulePositions(swerveDriveSimulation.getLatestModulePositions()); - aKitLog.record("RobotVelocity", swerveDriveSimulation.getActualSpeedsFieldRelative()); + var speeds = swerveDriveSimulation.getActualSpeedsFieldRelative(); + aKitLog.record("RobotVelocity",speeds); + aKitLog.record("RobotVelocityMagnitude", Math.sqrt(Math.pow(speeds.vxMetersPerSecond, 2) + + Math.pow(speeds.vyMetersPerSecond, 2))); // update gyro reading from sim ((MockGyro) pose.imu).setYaw(this.swerveDriveSimulation.getOdometryEstimatedPose().getRotation().getDegrees()); diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java index 31860b5a..31d27bf1 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java @@ -21,6 +21,8 @@ public class AlignToReefWithAprilTagCommand extends AlignToTagGlobalMovementWith private boolean isDriverRelative = false; private boolean hasCameraFlippedDriverRelative = false; private boolean hasSetConfiguration = false; + private boolean requireExcellentAlignment = true; + private AlignCameraToAprilTagCalculator.Activity startingActivity = AlignCameraToAprilTagCalculator.Activity.Searching; @Inject public AlignToReefWithAprilTagCommand(AprilTagVisionSubsystemExtended aprilTagVisionSubsystem, DriveSubsystem drive, diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToTagGlobalMovementWithCalculator.java b/src/main/java/competition/subsystems/drive/commands/AlignToTagGlobalMovementWithCalculator.java index e9a2c5e3..59f99d6b 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToTagGlobalMovementWithCalculator.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToTagGlobalMovementWithCalculator.java @@ -11,6 +11,8 @@ import javax.inject.Inject; +import java.util.function.Supplier; + import static edu.wpi.first.units.Units.Inches; public class AlignToTagGlobalMovementWithCalculator extends BaseCommand { @@ -20,7 +22,7 @@ public class AlignToTagGlobalMovementWithCalculator extends BaseCommand { final ElectricalContract electricalContract; final PoseSubsystem pose; - private int targetAprilTagID; + private Supplier targetAprilTagIdSupplier; private int targetCameraID; private boolean isCameraBackwards; private Distance offset; @@ -56,8 +58,20 @@ public void setConfigurations(int targetCameraID, int targetAprilTagID, boolean public void setConfigurations(int targetCameraID, int targetAprilTagID, boolean isCameraBackwards, double offsetInInches, AlignCameraToAprilTagCalculator.Activity startingActivity, boolean requireExcellentAlignment) { + setConfigurations( + targetCameraID, + () -> targetAprilTagID, + isCameraBackwards, + offsetInInches, + startingActivity, + requireExcellentAlignment + ); + } + + public void setConfigurations(int targetCameraID, Supplier targetAprilTagID, boolean isCameraBackwards, double offsetInInches, + AlignCameraToAprilTagCalculator.Activity startingActivity, boolean requireExcellentAlignment) { this.targetCameraID = targetCameraID; - this.targetAprilTagID = targetAprilTagID; + this.targetAprilTagIdSupplier = targetAprilTagID; this.isCameraBackwards = isCameraBackwards; this.offset = Inches.of(offsetInInches); this.hasSetConfiguration = true; @@ -73,7 +87,7 @@ public void initialize() { return; } - calculator.configureAndReset(targetAprilTagID, targetCameraID, offset, + calculator.configureAndReset(targetAprilTagIdSupplier.get(), targetCameraID, offset, isCameraBackwards, startingActivity, requireExcellentAlignment); pose.setPreferOdometryToVision(true); } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java new file mode 100644 index 00000000..c3c01c6b --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -0,0 +1,173 @@ +package competition.subsystems.drive.commands; + +import competition.motion.CommonRouteGenerator; +import competition.motion.CubicHermiteSpline; +import competition.motion.HermiteTrajectory; +import competition.operator_interface.OperatorInterface; +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import xbot.common.command.BaseCommand; +import xbot.common.math.XYPair; +import xbot.common.subsystems.drive.control_logic.HeadingModule; + +import javax.inject.Inject; +import java.util.ArrayList; + +public class DriveHermiteSplineCommand extends BaseCommand { + + final DriveSubsystem drive; + final PoseSubsystem pose; + final OperatorInterface oi; + final CommonRouteGenerator routeGenerator; + + HermiteTrajectory trajectory; + + private Landmarks.ReefFace endingReefFace; + private Landmarks.Branch endingBranch; + + private Landmarks.CoralStation endingCoralStation; + private double distanceToStartUsingFinalHeading = 0.33; + final HeadingModule headingModule; + Rotation2d initialHeading; + Rotation2d endingHeading; + + public enum TargetMode { + Reef, + Station + } + private TargetMode mode; + + @Inject + public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose, OperatorInterface oi, + CommonRouteGenerator routeGenerator, HeadingModule.HeadingModuleFactory headingModulefactory) { + this.drive = drive; + this.pose = pose; + this.oi = oi; + this.routeGenerator = routeGenerator; + trajectory = new HermiteTrajectory(); + mode = TargetMode.Reef; + + this.headingModule = headingModulefactory.create(drive.getRotateToHeadingPid()); + + addRequirements(drive); + } + + public void configureForReef(Landmarks.ReefFace reefFace, Landmarks.Branch branch) { + mode = TargetMode.Reef; + + endingReefFace = reefFace; + endingBranch = branch; + } + + public void configureForStation(Landmarks.CoralStation coralStation) { + mode = TargetMode.Station; + + endingCoralStation = coralStation; + } + + private void setDistanceToStartUsingFinalHeading(double distance) { + distanceToStartUsingFinalHeading = distance; + } + + @Override + public void initialize() { + log.info("Initializing"); + // In all cases we need the nearest loading station + var closestStation = pose.getClosestCoralStation(); + initialHeading = pose.getCurrentHeading(); + var route = new ArrayList(); + boolean flipFinalHeading = false; + + if (mode == TargetMode.Reef) { + route = routeGenerator.getRouteFromLoadingStationToReef( + closestStation, + endingReefFace, + endingBranch + ); + } else if (mode == TargetMode.Station) { + var closestFace = pose.getClosestReefFace(); + route = routeGenerator.getRouteFromReefToLoadingStation( + closestFace, + endingCoralStation); + flipFinalHeading = true; + } + + trajectory.setSplines(route); + endingHeading = route.get(route.size()-1).getEndControlVector().getAngle(); + if (flipFinalHeading) { + endingHeading = endingHeading.rotateBy(Rotation2d.fromDegrees(180)); + } + + trajectory.initialize(pose.getAbsoluteVelocity(), 1); + } + + @Override + public void execute() { + + var currentPosition = pose.getCurrentPose2d().getTranslation(); + var advice = trajectory.advise(currentPosition); + + // two components; drive based on pure velocity commands, and PID to ghost. + + // Velocity - spline will be outputting units of meters per second. Need to scale back into % of maximum + // output. + XYPair velocityIntent = new XYPair( + advice.velocity().getX() / drive.getMaxTargetSpeedMetersPerSecond(), + advice.velocity().getY() / drive.getMaxTargetSpeedMetersPerSecond()); + + aKitLog.record("HermiteVelocityRaw", velocityIntent.getMagnitude()); + + if (advice.timeFrozen()) { + // If the timer is frozen, that means we have gone off-track somewhere and need to catch back up. We have to + // be careful here - if we just set the velocity component to 0, we will use PID to approach, but as soon as we + // do the ghost will shoot ahead as it expects us to have much higher velocities. Instead, we need to use + // the portion of the velocity that is in the direction of the ghost. We can find the similarity of the vector + // using the dot product, and use that dot product to scale the velocity vector. + + var directionToGhost = advice.position().minus(currentPosition); + var directionToGhostUnitVector = XYPair.fromUnitPolar(directionToGhost.getAngle().getDegrees()); + var velocityIntentUnitVector = XYPair.fromUnitPolar(velocityIntent.getAngle()); + + var similarity = directionToGhostUnitVector.dotProduct(velocityIntentUnitVector); + velocityIntent = velocityIntent.clone().scale(similarity); + } + + aKitLog.record("HermiteVelocityAdjusted", velocityIntent.getMagnitude()); + + // Position - spline will be outputting a position that we should be at. We need to PID to that position. + var positionTranslation = drive.getPowerToAchieveFieldPosition(currentPosition, advice.position()); + XYPair positionIntent = new XYPair(positionTranslation.getX(), positionTranslation.getY()); + + var fusedIntent = velocityIntent.add(positionIntent); + + if (oi.driverGamepad.getLeftVector().getNorm() > 0.2) { + fusedIntent = new XYPair(oi.driverGamepad.getLeftVector().getX(), oi.driverGamepad.getLeftVector().getY()); + } + + if (fusedIntent.getMagnitude() > 1) { + fusedIntent = fusedIntent.clone().scale(1 / fusedIntent.getMagnitude()); + } + + double rotateIntent = 0; + if (advice.distanceTravelled() > distanceToStartUsingFinalHeading) { + rotateIntent = headingModule.calculateHeadingPower(endingHeading); + } + + drive.fieldOrientedDrive(fusedIntent, rotateIntent, pose.getCurrentHeading().getDegrees(), true); + + aKitLog.record("HermiteGhost", new Pose2d(advice.position(), new Rotation2d())); + + distanceRemaining = advice.timeRemaining(); + aKitLog.record("DistanceRemaining", distanceRemaining); + } + + double distanceRemaining; + + @Override + public boolean isFinished() { + return distanceRemaining < 0.05; + } +} diff --git a/src/main/java/competition/subsystems/drive/commands/ShoveCoralStationCommand.java b/src/main/java/competition/subsystems/drive/commands/ShoveCoralStationCommand.java index a41d0820..5bb7bb26 100644 --- a/src/main/java/competition/subsystems/drive/commands/ShoveCoralStationCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/ShoveCoralStationCommand.java @@ -1,8 +1,10 @@ package competition.subsystems.drive.commands; +import competition.subsystems.coral_scorer.CoralScorerSubsystem; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.wpilibj.DriverStation; import xbot.common.command.BaseCommand; import xbot.common.controls.sensors.XTimer; import xbot.common.math.XYPair; @@ -13,55 +15,63 @@ public class ShoveCoralStationCommand extends BaseCommand { - DriveSubsystem drive; - PoseSubsystem pose; + final DriveSubsystem drive; + final PoseSubsystem pose; + final CoralScorerSubsystem coralScorer; + PropertyFactory pf; + + // Properties final DoubleProperty shovePower; - final DoubleProperty shoveWaitTime; - private double startTime = -Double.MAX_VALUE; - double shoveAngleDegrees; + double shoveAngleDegreesRequested; + double shoveAngleActual; + @Inject - public ShoveCoralStationCommand(DriveSubsystem drive, PoseSubsystem pose, PropertyFactory pf) { + public ShoveCoralStationCommand(DriveSubsystem drive, PoseSubsystem pose, PropertyFactory pf, CoralScorerSubsystem coralScorer) { this.drive = drive; this.pose = pose; + this.coralScorer = coralScorer; + pf.setPrefix(this); shovePower = pf.createPersistentProperty("ShovePower", 0.25); - shoveWaitTime = pf.createPersistentProperty("ShoveWaitTime", 0.25); this.addRequirements(drive); } public void setShoveAngle(Landmarks.CoralStation coralStation) { if (coralStation == Landmarks.CoralStation.LEFT) { - shoveAngleDegrees = Landmarks.BlueLeftCoralStationMid.getRotation().getDegrees(); + shoveAngleDegreesRequested = Landmarks.BlueLeftCoralStationMid.getRotation().getDegrees(); } else { - shoveAngleDegrees = Landmarks.BlueRightCoralStationMid.getRotation().getDegrees(); + shoveAngleDegreesRequested = Landmarks.BlueRightCoralStationMid.getRotation().getDegrees(); } + // We want to shove in, default headings are "out" + shoveAngleDegreesRequested += 180; } @Override public void initialize() { log.info("Initializing"); - pose.setPreferOdometryToVision(true); - startTime = XTimer.getFPGATimestamp(); + shoveAngleActual = shoveAngleDegreesRequested; + if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red) { + shoveAngleActual = shoveAngleDegreesRequested + 180; + } } @Override public void execute() { - drive.fieldOrientedDrive(XYPair.fromPolar(shoveAngleDegrees, shovePower.get()), 0, pose.getCurrentHeading().getDegrees(), true); - aKitLog.record("ShoveAngleDegrees", shoveAngleDegrees); + drive.fieldOrientedDrive(XYPair.fromPolar(shoveAngleActual, shovePower.get()), 0, pose.getCurrentHeading().getDegrees(), true); + aKitLog.record("ShoveAngleDegrees", shoveAngleActual); } @Override public boolean isFinished() { - return XTimer.getFPGATimestamp() - startTime > shoveWaitTime.get(); + return coralScorer.confidentlyHasCoral(); } @Override public void end(boolean interrupted) { - pose.setPreferOdometryToVision(false); drive.stop(); } } diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java index 645cfb5e..d0579f64 100644 --- a/src/main/java/competition/subsystems/pose/Landmarks.java +++ b/src/main/java/competition/subsystems/pose/Landmarks.java @@ -240,4 +240,15 @@ public static List getAllianceReefFiducialIds(DriverStation.Alliance al return new ArrayList(); } } + + public static ReefFace mirrorReefFaceLeftToRight(ReefFace face) { + return switch (face) { + case CLOSE -> ReefFace.CLOSE; + case CLOSE_LEFT -> ReefFace.CLOSE_RIGHT; + case CLOSE_RIGHT -> ReefFace.CLOSE_LEFT; + case FAR -> ReefFace.FAR; + case FAR_LEFT -> ReefFace.FAR_RIGHT; + case FAR_RIGHT -> ReefFace.FAR_LEFT; + }; + } } diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 560a68fa..b3e04679 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -26,7 +26,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import org.kobe.xbot.JClient.XTablesClient; import org.kobe.xbot.Utilities.Entities.BatchedPushRequests; +import xbot.common.advantage.AKitLogger; import xbot.common.controls.sensors.XGyro.XGyroFactory; +import xbot.common.controls.sensors.XTimer; import xbot.common.math.WrappedRotation2d; import xbot.common.properties.BooleanProperty; import xbot.common.properties.Property; @@ -163,17 +165,35 @@ protected void updateOdometry() { totalDistanceX = robotPose.getX(); totalDistanceY = robotPose.getY(); - double prevTotalDistanceX = totalDistanceX; - double prevTotalDistanceY = totalDistanceY; this.velocityX = ((totalDistanceX - prevTotalDistanceX)); this.velocityY = ((totalDistanceY - prevTotalDistanceY)); - this.totalVelocity = (Math.sqrt(Math.pow(velocityX, 2.0) + Math.pow(velocityY, 2.0))); // Unnecessary? + double deltaTime = XTimer.getFPGATimestamp() - previousTimestamp; + if (deltaTime < 0.0001) { + // protecting against future divide by zero + deltaTime = 0.0001; + } + + prevTotalDistanceX = totalDistanceX; + prevTotalDistanceY = totalDistanceY; + previousTimestamp = XTimer.getFPGATimestamp(); + + aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); + + var motionThisTick = Math.sqrt(Math.pow(velocityX, 2.0) + Math.pow(velocityY, 2.0)); + + this.totalVelocity = motionThisTick / deltaTime; + aKitLog.record("TotalVelocity", this.totalVelocity); } + double prevTotalDistanceX; + double prevTotalDistanceY; + double previousTimestamp = 0; + public double getAbsoluteVelocity() { return this.totalVelocity; } + /** * Get a command that resets the pose estimator to the current vision estimate * @return The command that resets the pose estimator @@ -286,6 +306,29 @@ public Pose2d getClosestReefFacePose() { return currentPose.nearest(reefFacePoses); } + public Landmarks.ReefFace getClosestReefFace() { + Pose2d currentPose = getCurrentPose2d(); + var closeReefFace = convertBlueToRedIfNeeded(Landmarks.BlueCloseAlgae); + var closeLeftReefFace = convertBlueToRedIfNeeded(Landmarks.BlueCloseLeftAlgae); + var closeRightReefFace = convertBlueToRedIfNeeded(Landmarks.BlueCloseRightAlgae); + var farReefFace = convertBlueToRedIfNeeded(Landmarks.BlueFarAlgae); + var farLeftReefFace = convertBlueToRedIfNeeded(Landmarks.BlueFarLeftAlgae); + var farRightReefFace = convertBlueToRedIfNeeded(Landmarks.BlueFarRightAlgae); + + List reefFacePoses = Arrays.asList( + closeReefFace, closeLeftReefFace, closeRightReefFace, + farReefFace, farLeftReefFace, farRightReefFace); + HashMap hashMap = new HashMap<>(); + hashMap.put(closeReefFace, Landmarks.ReefFace.CLOSE); + hashMap.put(closeLeftReefFace, Landmarks.ReefFace.CLOSE_LEFT); + hashMap.put(closeRightReefFace, Landmarks.ReefFace.CLOSE_RIGHT); + hashMap.put(farReefFace, Landmarks.ReefFace.FAR); + hashMap.put(farLeftReefFace, Landmarks.ReefFace.FAR_LEFT); + hashMap.put(farRightReefFace, Landmarks.ReefFace.FAR_RIGHT); + + return hashMap.get(currentPose.nearest(reefFacePoses)); + } + public Landmarks.CoralStation getClosestCoralStation() { Pose2d currentPose = getCurrentPose2d(); var leftCoralStation = convertBlueToRedIfNeeded(Landmarks.BlueLeftCoralStationMid);