From d5600658736ad0dc6d2180319e7713990886bf28 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Wed, 5 Mar 2025 15:46:09 -0800 Subject: [PATCH 01/10] Thoughts on hermite splines --- .../motion/CubicHermiteSpline.java | 312 ++++++++++++++++++ .../motion/CubicHermiteSplineParameters.java | 6 + .../motion/CubicHermiteSplineVisualizer.java | 238 +++++++++++++ .../competition/motion/HermiteTrajectory.java | 66 ++++ .../motion/HermiteTrajectoryAdvice.java | 6 + .../motion/TrapezoidProfileAdvice.java | 4 + .../motion/TrapezoidProfileManager.java | 7 + .../OperatorCommandMap.java | 6 +- .../commands/DriveHermiteSplineCommand.java | 78 +++++ .../subsystems/pose/PoseSubsystem.java | 26 +- 10 files changed, 744 insertions(+), 5 deletions(-) create mode 100644 src/main/java/competition/motion/CubicHermiteSpline.java create mode 100644 src/main/java/competition/motion/CubicHermiteSplineParameters.java create mode 100644 src/main/java/competition/motion/CubicHermiteSplineVisualizer.java create mode 100644 src/main/java/competition/motion/HermiteTrajectory.java create mode 100644 src/main/java/competition/motion/HermiteTrajectoryAdvice.java create mode 100644 src/main/java/competition/motion/TrapezoidProfileAdvice.java create mode 100644 src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java diff --git a/src/main/java/competition/motion/CubicHermiteSpline.java b/src/main/java/competition/motion/CubicHermiteSpline.java new file mode 100644 index 00000000..9040d234 --- /dev/null +++ b/src/main/java/competition/motion/CubicHermiteSpline.java @@ -0,0 +1,312 @@ +package competition.motion; + + import edu.wpi.first.math.geometry.Translation2d; + + /** + * 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 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 starting control vector using a Translation2d. + */ + public void setStartControlVector(Translation2d vector) { + this.startControlVector = vector; + } + + /** + * 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(); } + } \ 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..6286fb81 --- /dev/null +++ b/src/main/java/competition/motion/CubicHermiteSplineParameters.java @@ -0,0 +1,6 @@ +package competition.motion; + +import edu.wpi.first.math.geometry.Translation2d; + +public record CubicHermiteSplineParameters(Translation2d startPoint, Translation2d endPoint, Translation2d startControlVector, Translation2d endControlVector) { +} diff --git a/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java b/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java new file mode 100644 index 00000000..17c6d122 --- /dev/null +++ b/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java @@ -0,0 +1,238 @@ +package competition.motion; + +import edu.wpi.first.math.geometry.Translation2d; + +import javax.swing.*; +import java.awt.*; +import java.awt.event.ActionEvent; +import java.awt.event.ActionListener; +import java.awt.geom.AffineTransform; +import java.awt.geom.Line2D; + +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..2131343f --- /dev/null +++ b/src/main/java/competition/motion/HermiteTrajectory.java @@ -0,0 +1,66 @@ +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; + +public class HermiteTrajectory { + + CubicHermiteSpline spline; + TrapezoidProfile trapezoid; + double arcLengthMeters; + double startTimeInSeconds = 0; + double elapsedSeconds = 0; + double frozenOffsetSeconds = 0; + double maxGap = 0.25; + private TrapezoidProfile.State initialState; + private TrapezoidProfile.State finalState; + + @Inject + public HermiteTrajectory() { + trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(2, 1)); + } + + public void setSpline(CubicHermiteSpline spline) { + this.spline = spline; + } + + public void initialize(double currentVelocityMetersPerSecond, double finalVelocityMetersPerSecond) { + arcLengthMeters = spline.estimateArcLength(); + startTimeInSeconds = XTimer.getFPGATimestamp(); + spline.initializeArcLengthTable(); + initialState = new TrapezoidProfile.State(0, currentVelocityMetersPerSecond); + finalState = new TrapezoidProfile.State(arcLengthMeters, finalVelocityMetersPerSecond); + frozenOffsetSeconds = 0; + elapsedSeconds = 0; + } + + public HermiteTrajectoryAdvice advise(Translation2d currentPosition) { + + double elapsedSecondsCandidate = XTimer.getFPGATimestamp() - startTimeInSeconds - frozenOffsetSeconds; + // check to see how far away we are from our projected point + var candidateAdvice = getAdviceForTime(elapsedSecondsCandidate, false); + + // Now do a quick check - if the positions are too far apart, fall back on the last time candidate. + if (Math.abs(candidateAdvice.position().getDistance(currentPosition)) > maxGap) { + // We need to freeze here until the robot catches up + 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 lerp = spline.getParameterFromDistance(trapezoidAdvice.position); + var position = spline.evaluate(lerp); + var direction = spline.derivative(lerp); + var velocity = new Translation2d(trapezoidAdvice.velocity, direction.getAngle()); + return new HermiteTrajectoryAdvice(position, velocity, timeFrozen); + } +} diff --git a/src/main/java/competition/motion/HermiteTrajectoryAdvice.java b/src/main/java/competition/motion/HermiteTrajectoryAdvice.java new file mode 100644 index 00000000..85f38c42 --- /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) { +} 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/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index ebad39b9..e89e1fa4 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -26,6 +26,7 @@ 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; @@ -91,7 +92,8 @@ public void setupDriverCommands( HeadingAssistedDriveAndScoreCommandGroup.Factory headingAssistedDriveAndScoreCommandGroupFactory, AlignToSpecificHumanLoadingStationCommand alignToLeftStation, DriveToNearestReefFaceWithPID driveToNearestReefFaceWithPID, - AlignToNearestCoralStationCommand alignToNearestCoralStationCommand) { + AlignToNearestCoralStationCommand alignToNearestCoralStationCommand, + DriveHermiteSplineCommand hermite) { resetHeading.setHeadingToApply(0); operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.Start).onTrue(resetHeading); @@ -113,7 +115,7 @@ public void setupDriverCommands( operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(typicalSwerveDrive); alignToLeftStation.setCoralStation(Landmarks.CoralStation.LEFT); - operatorInterface.driverGamepad.getPovIfAvailable(270).whileTrue(alignToLeftStation); + operatorInterface.driverGamepad.getPovIfAvailable(270).whileTrue(hermite); // operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.RightBumper).whileTrue(intakeCoralCommand); // operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.LeftBumper).whileTrue(scoreCoralCommand); 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..59bdfd96 --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -0,0 +1,78 @@ +package competition.subsystems.drive.commands; + +import competition.motion.CubicHermiteSpline; +import competition.motion.CubicHermiteSplineParameters; +import competition.motion.HermiteTrajectory; +import competition.motion.HermiteTrajectoryAdvice; +import competition.subsystems.drive.DriveSubsystem; +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 xbot.common.command.BaseCommand; +import xbot.common.math.XYPair; + +import javax.inject.Inject; + +public class DriveHermiteSplineCommand extends BaseCommand { + + final DriveSubsystem drive; + final PoseSubsystem pose; + + + HermiteTrajectory trajectory; + final Translation2d startPoint; + + @Inject + public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose) { + this.drive = drive; + this.pose = pose; + + trajectory = new HermiteTrajectory(); + CubicHermiteSpline spline = new CubicHermiteSpline(); + startPoint = new Translation2d(4.966,5.154); + spline.setStartPoint(startPoint); + spline.setEndPoint(new Translation2d(1.2,6.953)); + spline.setStartControlVector(5, Math.toRadians(60)); + spline.setEndControlVector(5, Math.toRadians(160)); + trajectory.setSpline(spline); + } + + public void setSplineParameters(CubicHermiteSplineParameters parameters) { + var spline = new CubicHermiteSpline(parameters); + trajectory.setSpline(spline); + } + + @Override + public void initialize() { + trajectory.initialize(pose.getAbsoluteVelocity(), 0.2); + } + + @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()); + if (advice.timeFrozen()) { + velocityIntent = new XYPair(0, 0); + } + + // 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); + + drive.fieldOrientedDrive(fusedIntent, 0, pose.getCurrentHeading().getDegrees(), true); + + aKitLog.record("HermiteGhost", new Pose2d(advice.position(), new Rotation2d())); + } +} diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 4e1f2d4d..79489695 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 From f7ac09a09c661add5f324e7b977d2e1e5c2cceb5 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Wed, 5 Mar 2025 16:36:26 -0800 Subject: [PATCH 02/10] small changes --- src/main/java/competition/motion/HermiteTrajectory.java | 2 +- .../subsystems/drive/commands/DriveHermiteSplineCommand.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/motion/HermiteTrajectory.java b/src/main/java/competition/motion/HermiteTrajectory.java index 2131343f..e9622074 100644 --- a/src/main/java/competition/motion/HermiteTrajectory.java +++ b/src/main/java/competition/motion/HermiteTrajectory.java @@ -20,7 +20,7 @@ public class HermiteTrajectory { @Inject public HermiteTrajectory() { - trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(2, 1)); + trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(2, 2)); } public void setSpline(CubicHermiteSpline spline) { diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index 59bdfd96..1a9e668a 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -61,10 +61,12 @@ public void execute() { XYPair velocityIntent = new XYPair( advice.velocity().getX() / drive.getMaxTargetSpeedMetersPerSecond(), advice.velocity().getY() / drive.getMaxTargetSpeedMetersPerSecond()); - if (advice.timeFrozen()) { + /*if (advice.timeFrozen()) { velocityIntent = new XYPair(0, 0); } + */ + // 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()); From 976c39ec4d7c1d90f7ea57506996fa4849ddee52 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 13:14:42 -0800 Subject: [PATCH 03/10] Allow for path recovery - also temporarily add joystick to induce bad paths. --- .../commands/DriveHermiteSplineCommand.java | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index 1a9e668a..cc8f472c 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -4,6 +4,8 @@ import competition.motion.CubicHermiteSplineParameters; import competition.motion.HermiteTrajectory; import competition.motion.HermiteTrajectoryAdvice; +import competition.operator_interface.OperatorCommandMap; +import competition.operator_interface.OperatorInterface; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -18,15 +20,17 @@ public class DriveHermiteSplineCommand extends BaseCommand { final DriveSubsystem drive; final PoseSubsystem pose; + final OperatorInterface oi; HermiteTrajectory trajectory; final Translation2d startPoint; @Inject - public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose) { + public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose, OperatorInterface oi) { this.drive = drive; this.pose = pose; + this.oi = oi; trajectory = new HermiteTrajectory(); CubicHermiteSpline spline = new CubicHermiteSpline(); @@ -61,18 +65,31 @@ public void execute() { XYPair velocityIntent = new XYPair( advice.velocity().getX() / drive.getMaxTargetSpeedMetersPerSecond(), advice.velocity().getY() / drive.getMaxTargetSpeedMetersPerSecond()); - /*if (advice.timeFrozen()) { - velocityIntent = new XYPair(0, 0); + 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); } - */ - // 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()); + } + drive.fieldOrientedDrive(fusedIntent, 0, pose.getCurrentHeading().getDegrees(), true); aKitLog.record("HermiteGhost", new Pose2d(advice.position(), new Rotation2d())); From c2f3281ec5f66bfa89873b32d2c1f20b329f4f5d Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 16:27:37 -0800 Subject: [PATCH 04/10] Incorporate linked splines --- .../competition/motion/HermiteTrajectory.java | 173 +++++++++++------- .../motion/HermiteTrajectoryAdvice.java | 2 +- .../commands/DriveHermiteSplineCommand.java | 28 ++- 3 files changed, 136 insertions(+), 67 deletions(-) diff --git a/src/main/java/competition/motion/HermiteTrajectory.java b/src/main/java/competition/motion/HermiteTrajectory.java index e9622074..f59aa8cb 100644 --- a/src/main/java/competition/motion/HermiteTrajectory.java +++ b/src/main/java/competition/motion/HermiteTrajectory.java @@ -1,66 +1,115 @@ 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; - -public class HermiteTrajectory { - - CubicHermiteSpline spline; - TrapezoidProfile trapezoid; - double arcLengthMeters; - double startTimeInSeconds = 0; - double elapsedSeconds = 0; - double frozenOffsetSeconds = 0; - double maxGap = 0.25; - private TrapezoidProfile.State initialState; - private TrapezoidProfile.State finalState; - - @Inject - public HermiteTrajectory() { - trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(2, 2)); - } - - public void setSpline(CubicHermiteSpline spline) { - this.spline = spline; - } - - public void initialize(double currentVelocityMetersPerSecond, double finalVelocityMetersPerSecond) { - arcLengthMeters = spline.estimateArcLength(); - startTimeInSeconds = XTimer.getFPGATimestamp(); - spline.initializeArcLengthTable(); - initialState = new TrapezoidProfile.State(0, currentVelocityMetersPerSecond); - finalState = new TrapezoidProfile.State(arcLengthMeters, finalVelocityMetersPerSecond); - frozenOffsetSeconds = 0; - elapsedSeconds = 0; - } - - public HermiteTrajectoryAdvice advise(Translation2d currentPosition) { - - double elapsedSecondsCandidate = XTimer.getFPGATimestamp() - startTimeInSeconds - frozenOffsetSeconds; - // check to see how far away we are from our projected point - var candidateAdvice = getAdviceForTime(elapsedSecondsCandidate, false); - - // Now do a quick check - if the positions are too far apart, fall back on the last time candidate. - if (Math.abs(candidateAdvice.position().getDistance(currentPosition)) > maxGap) { - // We need to freeze here until the robot catches up - candidateAdvice = getAdviceForTime(elapsedSeconds, true); - frozenOffsetSeconds = XTimer.getFPGATimestamp() - startTimeInSeconds - elapsedSeconds; - } else { - elapsedSeconds = elapsedSecondsCandidate; + 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.25; + 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, 3)); + 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; } - return candidateAdvice; - } - - private HermiteTrajectoryAdvice getAdviceForTime(double elapsedSeconds, boolean timeFrozen) { - var trapezoidAdvice = trapezoid.calculate(elapsedSeconds, initialState, finalState); - double lerp = spline.getParameterFromDistance(trapezoidAdvice.position); - var position = spline.evaluate(lerp); - var direction = spline.derivative(lerp); - var velocity = new Translation2d(trapezoidAdvice.velocity, direction.getAngle()); - return new HermiteTrajectoryAdvice(position, velocity, timeFrozen); - } -} + 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, trapezoid.timeLeftUntil(finalState.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 index 85f38c42..abcf6f93 100644 --- a/src/main/java/competition/motion/HermiteTrajectoryAdvice.java +++ b/src/main/java/competition/motion/HermiteTrajectoryAdvice.java @@ -2,5 +2,5 @@ import edu.wpi.first.math.geometry.Translation2d; -public record HermiteTrajectoryAdvice(Translation2d position, Translation2d velocity, boolean timeFrozen) { +public record HermiteTrajectoryAdvice(Translation2d position, Translation2d velocity, boolean timeFrozen, double timeRemaining) { } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index cc8f472c..23818d84 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -15,6 +15,9 @@ import xbot.common.math.XYPair; import javax.inject.Inject; +import java.sql.Array; +import java.util.ArrayList; +import java.util.Collections; public class DriveHermiteSplineCommand extends BaseCommand { @@ -40,11 +43,28 @@ public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose, Opera spline.setStartControlVector(5, Math.toRadians(60)); spline.setEndControlVector(5, Math.toRadians(160)); trajectory.setSpline(spline); - } - public void setSplineParameters(CubicHermiteSplineParameters parameters) { - var spline = new CubicHermiteSpline(parameters); - trajectory.setSpline(spline); + var splineAParams = new CubicHermiteSplineParameters( + new Translation2d(1, 1), + new Translation2d(5, 1), + new Translation2d(5, Rotation2d.fromDegrees(0)), + new Translation2d(5, Rotation2d.fromDegrees(0)) + ); + var splineBParams = new CubicHermiteSplineParameters( + new Translation2d(5, 1), + new Translation2d(7, 7), + new Translation2d(5, Rotation2d.fromDegrees(0)), + new Translation2d(5, Rotation2d.fromDegrees(90)) + ); + + var splineA = new CubicHermiteSpline(splineAParams); + var splineB = new CubicHermiteSpline(splineBParams); + var splineList = new ArrayList(); + splineList.add(splineA); + splineList.add(splineB); + trajectory.setSplines(splineList); + + addRequirements(drive); } @Override From 4ccc0cd90449aa1f07c50e55ddeb90963a9b84c4 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 19:00:46 -0800 Subject: [PATCH 05/10] Handle red vs blue --- .../motion/CommonRouteGenerator.java | 133 ++++++++++++++++++ .../motion/CubicHermiteSpline.java | 25 +++- .../motion/CubicHermiteSplineParameters.java | 3 + .../competition/motion/HermiteTrajectory.java | 7 +- .../simulation/MapleSimulator.java | 5 +- .../commands/DriveHermiteSplineCommand.java | 32 ++++- .../subsystems/pose/Landmarks.java | 11 ++ .../subsystems/pose/PoseSubsystem.java | 23 +++ 8 files changed, 230 insertions(+), 9 deletions(-) create mode 100644 src/main/java/competition/motion/CommonRouteGenerator.java diff --git a/src/main/java/competition/motion/CommonRouteGenerator.java b/src/main/java/competition/motion/CommonRouteGenerator.java new file mode 100644 index 00000000..6ac94538 --- /dev/null +++ b/src/main/java/competition/motion/CommonRouteGenerator.java @@ -0,0 +1,133 @@ +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; + +@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 + + public List getRouteFromReefToLoadingStation( + Landmarks.ReefFace startingFace, + Landmarks.CoralStation station) { + + // 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. + + if (station == Landmarks.CoralStation.RIGHT) { + startingFace = Landmarks.mirrorReefFaceLeftToRight(startingFace); + } + + 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( + pose.getCurrentPose2d().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( + pose.getCurrentPose2d().getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(new Translation2d(4.5, 5.7)), + 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( + pose.getCurrentPose2d().getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(new Translation2d(6.2, 4)), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(-60))), + new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(90))) + ); + phase2 = new CubicHermiteSplineParameters( + phase1.endPoint(), + PoseSubsystem.convertBlueToRedIfNeeded(new Translation2d(4.5, 5.7)), + 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 (station == 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 index 9040d234..1ff4eedf 100644 --- a/src/main/java/competition/motion/CubicHermiteSpline.java +++ b/src/main/java/competition/motion/CubicHermiteSpline.java @@ -1,8 +1,14 @@ 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. */ @@ -309,4 +315,21 @@ public void initializeArcLengthTable() { 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(),PoseSubsystem.mirrorYCoordinateAcrossMidfield(spline.getStartControlVectorY())), + new Translation2d(spline.getEndControlVectorX(),PoseSubsystem.mirrorYCoordinateAcrossMidfield(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 index 6286fb81..85458212 100644 --- a/src/main/java/competition/motion/CubicHermiteSplineParameters.java +++ b/src/main/java/competition/motion/CubicHermiteSplineParameters.java @@ -3,4 +3,7 @@ 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/HermiteTrajectory.java b/src/main/java/competition/motion/HermiteTrajectory.java index f59aa8cb..13964091 100644 --- a/src/main/java/competition/motion/HermiteTrajectory.java +++ b/src/main/java/competition/motion/HermiteTrajectory.java @@ -15,7 +15,7 @@ public class HermiteTrajectory { private double startTimeInSeconds = 0; private double elapsedSeconds = 0; private double frozenOffsetSeconds = 0; - private final double maxGap = 0.25; + private final double maxGap = 0.75; private TrapezoidProfile.State initialState; private TrapezoidProfile.State finalState; @@ -24,7 +24,7 @@ public class HermiteTrajectory { @Inject public HermiteTrajectory() { - trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(4, 3)); + trapezoid = new TrapezoidProfile(new TrapezoidProfile.Constraints(4.5, 6)); splines = new ArrayList<>(); } @@ -110,6 +110,7 @@ private HermiteTrajectoryAdvice getAdviceForTime(double elapsedSeconds, boolean var direction = currentSpline.derivative(lerp); var velocity = new Translation2d(trapezoidAdvice.velocity, direction.getAngle()); - return new HermiteTrajectoryAdvice(position, velocity, timeFrozen, trapezoid.timeLeftUntil(finalState.position)); + return new HermiteTrajectoryAdvice(position, velocity, timeFrozen, + finalState.position - totalDistance); } } \ No newline at end of file diff --git a/src/main/java/competition/simulation/MapleSimulator.java b/src/main/java/competition/simulation/MapleSimulator.java index 5c913699..7c9d0016 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/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index 23818d84..61073a61 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -1,5 +1,6 @@ package competition.subsystems.drive.commands; +import competition.motion.CommonRouteGenerator; import competition.motion.CubicHermiteSpline; import competition.motion.CubicHermiteSplineParameters; import competition.motion.HermiteTrajectory; @@ -7,6 +8,7 @@ import competition.operator_interface.OperatorCommandMap; 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; @@ -24,16 +26,18 @@ public class DriveHermiteSplineCommand extends BaseCommand { final DriveSubsystem drive; final PoseSubsystem pose; final OperatorInterface oi; - + final CommonRouteGenerator routeGenerator; HermiteTrajectory trajectory; final Translation2d startPoint; @Inject - public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose, OperatorInterface oi) { + public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose, OperatorInterface oi, + CommonRouteGenerator routeGenerator) { this.drive = drive; this.pose = pose; this.oi = oi; + this.routeGenerator = routeGenerator; trajectory = new HermiteTrajectory(); CubicHermiteSpline spline = new CubicHermiteSpline(); @@ -62,14 +66,19 @@ public DriveHermiteSplineCommand(DriveSubsystem drive, PoseSubsystem pose, Opera var splineList = new ArrayList(); splineList.add(splineA); splineList.add(splineB); - trajectory.setSplines(splineList); addRequirements(drive); } @Override public void initialize() { - trajectory.initialize(pose.getAbsoluteVelocity(), 0.2); + + var closestFace = pose.getClosestReefFace(); + + var route = routeGenerator.getRouteFromReefToLoadingStation(closestFace, Landmarks.CoralStation.LEFT); + trajectory.setSplines(route); + + trajectory.initialize(pose.getAbsoluteVelocity(), 1); } @Override @@ -85,6 +94,9 @@ public void execute() { 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 @@ -100,6 +112,8 @@ public void execute() { 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()); @@ -113,5 +127,15 @@ public void execute() { drive.fieldOrientedDrive(fusedIntent, 0, pose.getCurrentHeading().getDegrees(), true); aKitLog.record("HermiteGhost", new Pose2d(advice.position(), new Rotation2d())); + + timeLeft = advice.timeRemaining(); + aKitLog.record("TimeLeft", timeLeft); + } + + double timeLeft; + + @Override + public boolean isFinished() { + return timeLeft < 0.05; } } 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 b250c9b6..b3e04679 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -306,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); From 33da703f7136415948c4163ecec3599ff0770b86 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 19:24:02 -0800 Subject: [PATCH 06/10] more bugfixes --- .../motion/CommonRouteGenerator.java | 22 ++++++++++++++----- .../motion/CubicHermiteSpline.java | 4 ++-- .../commands/DriveHermiteSplineCommand.java | 2 +- 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/java/competition/motion/CommonRouteGenerator.java b/src/main/java/competition/motion/CommonRouteGenerator.java index 6ac94538..c99a246b 100644 --- a/src/main/java/competition/motion/CommonRouteGenerator.java +++ b/src/main/java/competition/motion/CommonRouteGenerator.java @@ -10,6 +10,7 @@ import javax.inject.Inject; import javax.inject.Singleton; +import javax.swing.tree.TreeNode; import java.util.ArrayList; import java.util.List; @@ -31,6 +32,9 @@ public CommonRouteGenerator(ReefCoordinateGenerator reefCoordinateGenerator, Pos // 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 List getRouteFromReefToLoadingStation( Landmarks.ReefFace startingFace, Landmarks.CoralStation station) { @@ -40,8 +44,14 @@ public List getRouteFromReefToLoadingStation( // 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 (station == 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(); @@ -57,7 +67,7 @@ public List getRouteFromReefToLoadingStation( case CLOSE_RIGHT: // All of these work with the basic "get away from face then go straight to station" phase1 = new CubicHermiteSplineParameters( - pose.getCurrentPose2d().getTranslation(), + currentPose.getTranslation(), loadingStationEndPose.getTranslation(), new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.getReefFacePose(startingFace)).getRotation() .plus(new Rotation2d(Math.PI))), @@ -69,8 +79,8 @@ public List getRouteFromReefToLoadingStation( case FAR: // This one needs a little more work to get around the reef phase1 = new CubicHermiteSplineParameters( - pose.getCurrentPose2d().getTranslation(), - PoseSubsystem.convertBlueToRedIfNeeded(new Translation2d(4.5, 5.7)), + currentPose.getTranslation(), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustLeftOfReef), new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(45))), new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180))) ); @@ -87,14 +97,14 @@ public List getRouteFromReefToLoadingStation( break; case FAR_RIGHT: phase1 = new CubicHermiteSplineParameters( - pose.getCurrentPose2d().getTranslation(), - PoseSubsystem.convertBlueToRedIfNeeded(new Translation2d(6.2, 4)), + 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(new Translation2d(4.5, 5.7)), + PoseSubsystem.convertBlueToRedIfNeeded(pointJustLeftOfReef), phase1.endControlVector(), new Translation2d(5, PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180))) ); diff --git a/src/main/java/competition/motion/CubicHermiteSpline.java b/src/main/java/competition/motion/CubicHermiteSpline.java index 1ff4eedf..89a91369 100644 --- a/src/main/java/competition/motion/CubicHermiteSpline.java +++ b/src/main/java/competition/motion/CubicHermiteSpline.java @@ -323,8 +323,8 @@ public static CubicHermiteSpline mirrorSplineLeftToRight(CubicHermiteSpline spli 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(),PoseSubsystem.mirrorYCoordinateAcrossMidfield(spline.getStartControlVectorY())), - new Translation2d(spline.getEndControlVectorX(),PoseSubsystem.mirrorYCoordinateAcrossMidfield(spline.getEndControlVectorY())) + new Translation2d(spline.getStartControlVectorX(), -spline.getStartControlVectorY()), + new Translation2d(spline.getEndControlVectorX(), -spline.getEndControlVectorY()) )); } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index 61073a61..e2ab0530 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -75,7 +75,7 @@ public void initialize() { var closestFace = pose.getClosestReefFace(); - var route = routeGenerator.getRouteFromReefToLoadingStation(closestFace, Landmarks.CoralStation.LEFT); + var route = routeGenerator.getRouteFromReefToLoadingStation(closestFace, Landmarks.CoralStation.RIGHT); trajectory.setSplines(route); trajectory.initialize(pose.getAbsoluteVelocity(), 1); From f08d41a3ca537761221ef54c30cdf1476f39891a Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 22:34:07 -0800 Subject: [PATCH 07/10] More progress, moving towards unified scoring --- ...riveToFaceAndScoreCommandGroupFactory.java | 11 +- ...oReefFaceThenAlignCommandGroupFactory.java | 43 ++++--- .../motion/CommonRouteGenerator.java | 107 +++++++++++++++- .../NeoTrellisSubsystem.java | 59 ++++++--- .../OperatorCommandMap.java | 61 ++++++++- .../AlignToReefWithAprilTagCommand.java | 2 + ...lignToTagGlobalMovementWithCalculator.java | 20 ++- .../commands/DriveHermiteSplineCommand.java | 117 +++++++++++------- 8 files changed, 330 insertions(+), 90 deletions(-) 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 index c99a246b..fb95211c 100644 --- a/src/main/java/competition/motion/CommonRouteGenerator.java +++ b/src/main/java/competition/motion/CommonRouteGenerator.java @@ -10,10 +10,11 @@ import javax.inject.Inject; import javax.inject.Singleton; -import javax.swing.tree.TreeNode; import java.util.ArrayList; import java.util.List; +import static edu.wpi.first.units.Units.Meters; + @Singleton public class CommonRouteGenerator { @@ -35,9 +36,9 @@ public CommonRouteGenerator(ReefCoordinateGenerator reefCoordinateGenerator, Pos Translation2d pointJustLeftOfReef = new Translation2d(4.5, 5.7); Translation2d pointJustAboveReef = new Translation2d(6.2, 4); - public List getRouteFromReefToLoadingStation( + public ArrayList getRouteFromReefToLoadingStation( Landmarks.ReefFace startingFace, - Landmarks.CoralStation station) { + 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 @@ -46,7 +47,7 @@ public List getRouteFromReefToLoadingStation( var currentPose = pose.getCurrentPose2d(); - if (station == Landmarks.CoralStation.RIGHT) { + if (endingStation == Landmarks.CoralStation.RIGHT) { startingFace = Landmarks.mirrorReefFaceLeftToRight(startingFace); // Pretend the robot is on the other side as well. currentPose = new Pose2d( @@ -124,7 +125,103 @@ public List getRouteFromReefToLoadingStation( } - if (station == Landmarks.CoralStation.RIGHT) { + 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); } 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 3d2f865e..ee90c71d 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; @@ -45,9 +47,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; @@ -113,7 +118,7 @@ public void setupDriverCommands( operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(typicalSwerveDrive); - operatorInterface.driverGamepad.getPovIfAvailable(270).whileTrue(hermite); + //operatorInterface.driverGamepad.getPovIfAvailable(270).whileTrue(hermite); // (BLUE ALLIANCE) Below are different routes to test the SwerveSimpleTrajectoryCommand // I don't think createPotentiallyFilppedXbotSwervePoint works under OperatorCommandMap @@ -320,6 +325,60 @@ 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, + AutonomousCommandSelector selector, + SmartDashboardCommandPutter commandPutter, + DriveToFaceAndScoreCommandGroupFactory driveToFaceAndScoreFactory) { + + var collectLeft = splineDriveProvider.get(); + collectLeft.configureForStation(Landmarks.CoralStation.LEFT); + collectLeft.includeOnSmartDashboard("Collect Left"); + + 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); + actuallyScoreClose.addCommands(splineToClose, womboCombo); + SmartDashboard.putData("ActuallyScoreClose", actuallyScoreClose); + + oi.driverGamepad.getPovIfAvailable(270).whileTrue(actuallyScoreClose); + } + @Inject public void setupSimulatorCommands( ResetSimulatedPose resetPose 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 index e2ab0530..183d02db 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -2,24 +2,19 @@ import competition.motion.CommonRouteGenerator; import competition.motion.CubicHermiteSpline; -import competition.motion.CubicHermiteSplineParameters; import competition.motion.HermiteTrajectory; -import competition.motion.HermiteTrajectoryAdvice; -import competition.operator_interface.OperatorCommandMap; 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 edu.wpi.first.math.geometry.Translation2d; import xbot.common.command.BaseCommand; import xbot.common.math.XYPair; +import xbot.common.subsystems.drive.control_logic.HeadingModule; import javax.inject.Inject; -import java.sql.Array; import java.util.ArrayList; -import java.util.Collections; public class DriveHermiteSplineCommand extends BaseCommand { @@ -29,54 +24,81 @@ public class DriveHermiteSplineCommand extends BaseCommand { final CommonRouteGenerator routeGenerator; HermiteTrajectory trajectory; - final Translation2d startPoint; + + private Landmarks.ReefFace endingReefFace; + private Landmarks.Branch endingBranch; + + private Landmarks.CoralStation endingCoralStation; + private double distanceToStartUsingFinalHeading = 2.5; + final private 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) { + CommonRouteGenerator routeGenerator, HeadingModule.HeadingModuleFactory headingModulefactory) { this.drive = drive; this.pose = pose; this.oi = oi; this.routeGenerator = routeGenerator; - trajectory = new HermiteTrajectory(); - CubicHermiteSpline spline = new CubicHermiteSpline(); - startPoint = new Translation2d(4.966,5.154); - spline.setStartPoint(startPoint); - spline.setEndPoint(new Translation2d(1.2,6.953)); - spline.setStartControlVector(5, Math.toRadians(60)); - spline.setEndControlVector(5, Math.toRadians(160)); - trajectory.setSpline(spline); - - var splineAParams = new CubicHermiteSplineParameters( - new Translation2d(1, 1), - new Translation2d(5, 1), - new Translation2d(5, Rotation2d.fromDegrees(0)), - new Translation2d(5, Rotation2d.fromDegrees(0)) - ); - var splineBParams = new CubicHermiteSplineParameters( - new Translation2d(5, 1), - new Translation2d(7, 7), - new Translation2d(5, Rotation2d.fromDegrees(0)), - new Translation2d(5, Rotation2d.fromDegrees(90)) - ); - - var splineA = new CubicHermiteSpline(splineAParams); - var splineB = new CubicHermiteSpline(splineBParams); - var splineList = new ArrayList(); - splineList.add(splineA); - splineList.add(splineB); + 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 seconds) { + distanceToStartUsingFinalHeading = seconds; + } + @Override public void initialize() { + // 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; + } - var closestFace = pose.getClosestReefFace(); - - var route = routeGenerator.getRouteFromReefToLoadingStation(closestFace, Landmarks.CoralStation.RIGHT); trajectory.setSplines(route); + endingHeading = route.get(route.size()-1).getEndControlVector().getAngle(); + if (flipFinalHeading) { + endingHeading = endingHeading.rotateBy(Rotation2d.fromDegrees(180)); + } trajectory.initialize(pose.getAbsoluteVelocity(), 1); } @@ -124,18 +146,27 @@ public void execute() { fusedIntent = new XYPair(oi.driverGamepad.getLeftVector().getX(), oi.driverGamepad.getLeftVector().getY()); } - drive.fieldOrientedDrive(fusedIntent, 0, pose.getCurrentHeading().getDegrees(), true); + if (fusedIntent.getMagnitude() > 1) { + fusedIntent = fusedIntent.clone().scale(1 / fusedIntent.getMagnitude()); + } + + double rotateIntent = 0; + if (advice.timeRemaining() < distanceToStartUsingFinalHeading) { + rotateIntent = headingModule.calculateHeadingPower(endingHeading); + } + + drive.fieldOrientedDrive(fusedIntent, rotateIntent, pose.getCurrentHeading().getDegrees(), true); aKitLog.record("HermiteGhost", new Pose2d(advice.position(), new Rotation2d())); - timeLeft = advice.timeRemaining(); - aKitLog.record("TimeLeft", timeLeft); + distanceRemaining = advice.timeRemaining(); + aKitLog.record("DistanceRemaining", distanceRemaining); } - double timeLeft; + double distanceRemaining; @Override public boolean isFinished() { - return timeLeft < 0.05; + return distanceRemaining < 0.05; } } From bd11b24eb2540404ebcdebc347d353042fdf1f69 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 22:48:20 -0800 Subject: [PATCH 08/10] Saw some points --- SeriouslyCommonLib | 2 +- .../competition/operator_interface/OperatorCommandMap.java | 5 ++--- .../subsystems/drive/commands/DriveHermiteSplineCommand.java | 1 + 3 files changed, 4 insertions(+), 4 deletions(-) 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/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index ee90c71d..2783b5b5 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -373,10 +373,9 @@ public void setupDirectRouteCommands( var womboCombo = driveToFaceAndScoreFactory.create( Landmarks.ReefFace.CLOSE, Landmarks.Branch.A, Landmarks.CoralLevel.FOUR, true); - actuallyScoreClose.addCommands(splineToClose, womboCombo); - SmartDashboard.putData("ActuallyScoreClose", actuallyScoreClose); + SmartDashboard.putData("ActuallyScoreClose", womboCombo); - oi.driverGamepad.getPovIfAvailable(270).whileTrue(actuallyScoreClose); + oi.driverGamepad.getPovIfAvailable(270).whileTrue(womboCombo); } @Inject diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index 183d02db..e56722ca 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -74,6 +74,7 @@ private void setDistanceToStartUsingFinalHeading(double seconds) { @Override public void initialize() { + log.info("Initializing"); // In all cases we need the nearest loading station var closestStation = pose.getClosestCoralStation(); initialHeading = pose.getCurrentHeading(); From 2cc45a274cd1d0ea019b1d95cb3f5447bde982d1 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 23:17:23 -0800 Subject: [PATCH 09/10] Scoring and collecting together --- .../competition/motion/HermiteTrajectory.java | 2 +- .../motion/HermiteTrajectoryAdvice.java | 2 +- .../OperatorCommandMap.java | 13 +++++- .../commands/DriveHermiteSplineCommand.java | 8 ++-- .../commands/ShoveCoralStationCommand.java | 40 ++++++++++++------- 5 files changed, 42 insertions(+), 23 deletions(-) diff --git a/src/main/java/competition/motion/HermiteTrajectory.java b/src/main/java/competition/motion/HermiteTrajectory.java index 13964091..e7f56a8e 100644 --- a/src/main/java/competition/motion/HermiteTrajectory.java +++ b/src/main/java/competition/motion/HermiteTrajectory.java @@ -111,6 +111,6 @@ private HermiteTrajectoryAdvice getAdviceForTime(double elapsedSeconds, boolean var velocity = new Translation2d(trapezoidAdvice.velocity, direction.getAngle()); return new HermiteTrajectoryAdvice(position, velocity, timeFrozen, - finalState.position - totalDistance); + 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 index abcf6f93..529233e7 100644 --- a/src/main/java/competition/motion/HermiteTrajectoryAdvice.java +++ b/src/main/java/competition/motion/HermiteTrajectoryAdvice.java @@ -2,5 +2,5 @@ import edu.wpi.first.math.geometry.Translation2d; -public record HermiteTrajectoryAdvice(Translation2d position, Translation2d velocity, boolean timeFrozen, double timeRemaining) { +public record HermiteTrajectoryAdvice(Translation2d position, Translation2d velocity, boolean timeFrozen, double timeRemaining, double distanceTravelled) { } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 2783b5b5..f85f1746 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -35,6 +35,7 @@ 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; @@ -330,13 +331,21 @@ public void setupDirectRouteCommands( OperatorInterface oi, Provider splineDriveProvider, Provider alignToReefWithAprilTagProvider, + Provider shoveStationProvider, AutonomousCommandSelector selector, SmartDashboardCommandPutter commandPutter, - DriveToFaceAndScoreCommandGroupFactory driveToFaceAndScoreFactory) { + Provider intakeCoralCommandProvider, + DriveToFaceAndScoreCommandGroupFactory driveToFaceAndScoreFactory, + PrepCoralSystemCommandGroupFactory prepCoralSystemCommandGroupFactory) { var collectLeft = splineDriveProvider.get(); collectLeft.configureForStation(Landmarks.CoralStation.LEFT); - collectLeft.includeOnSmartDashboard("Collect 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); diff --git a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index e56722ca..e90895fc 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -29,7 +29,7 @@ public class DriveHermiteSplineCommand extends BaseCommand { private Landmarks.Branch endingBranch; private Landmarks.CoralStation endingCoralStation; - private double distanceToStartUsingFinalHeading = 2.5; + private double distanceToStartUsingFinalHeading = 0.33; final private HeadingModule headingModule; Rotation2d initialHeading; Rotation2d endingHeading; @@ -68,8 +68,8 @@ public void configureForStation(Landmarks.CoralStation coralStation) { endingCoralStation = coralStation; } - private void setDistanceToStartUsingFinalHeading(double seconds) { - distanceToStartUsingFinalHeading = seconds; + private void setDistanceToStartUsingFinalHeading(double distance) { + distanceToStartUsingFinalHeading = distance; } @Override @@ -152,7 +152,7 @@ public void execute() { } double rotateIntent = 0; - if (advice.timeRemaining() < distanceToStartUsingFinalHeading) { + if (advice.distanceTravelled() > distanceToStartUsingFinalHeading) { rotateIntent = headingModule.calculateHeadingPower(endingHeading); } 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(); } } From b1ea8c2a90b0d5b1b71ca53a15d63b84596e3a67 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 7 Mar 2025 23:29:36 -0800 Subject: [PATCH 10/10] Appease checkstyle --- .idea/codeStyles/Project.xml | 3 ++ .../motion/CubicHermiteSpline.java | 46 +++++++++---------- .../motion/CubicHermiteSplineVisualizer.java | 19 ++++++-- .../simulation/MapleSimulator.java | 4 +- .../commands/DriveHermiteSplineCommand.java | 2 +- 5 files changed, 44 insertions(+), 30 deletions(-) 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/src/main/java/competition/motion/CubicHermiteSpline.java b/src/main/java/competition/motion/CubicHermiteSpline.java index 89a91369..1cf78985 100644 --- a/src/main/java/competition/motion/CubicHermiteSpline.java +++ b/src/main/java/competition/motion/CubicHermiteSpline.java @@ -63,6 +63,13 @@ public void setStartControlVector(double magnitude, double 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. */ @@ -72,13 +79,6 @@ public void setEndControlVector(double magnitude, double directionRadians) { this.endControlVector = 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 a Translation2d. */ @@ -103,10 +103,10 @@ public Translation2d evaluate(double 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(); + 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); } @@ -121,8 +121,8 @@ public double evaluateX(double 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(); + return h00 * startPoint.getX() + h10 * startControlVector.getX() + + h01 * endPoint.getX() + h11 * endControlVector.getX(); } /** @@ -135,8 +135,8 @@ public double evaluateY(double 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(); + return h00 * startPoint.getY() + h10 * startControlVector.getY() + + h01 * endPoint.getY() + h11 * endControlVector.getY(); } /** @@ -148,10 +148,10 @@ public Translation2d derivative(double t) { 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(); + 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); } @@ -165,8 +165,8 @@ public double derivativeX(double t) { 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(); + return dh00 * startPoint.getX() + dh10 * startControlVector.getX() + + dh01 * endPoint.getX() + dh11 * endControlVector.getX(); } /** @@ -178,8 +178,8 @@ public double derivativeY(double t) { 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(); + return dh00 * startPoint.getY() + dh10 * startControlVector.getY() + + dh01 * endPoint.getY() + dh11 * endControlVector.getY(); } /** diff --git a/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java b/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java index 17c6d122..a6016d92 100644 --- a/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java +++ b/src/main/java/competition/motion/CubicHermiteSplineVisualizer.java @@ -2,12 +2,21 @@ import edu.wpi.first.math.geometry.Translation2d; -import javax.swing.*; -import java.awt.*; +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; -import java.awt.geom.Line2D; + public class CubicHermiteSplineVisualizer extends JFrame { @@ -59,7 +68,9 @@ public CubicHermiteSplineVisualizer() { public void actionPerformed(ActionEvent e) { if (currentT < 1.0) { currentT += ANIMATION_SPEED; - if (currentT > 1.0) currentT = 1.0; + if (currentT > 1.0) { + currentT = 1.0; + } drawPanel.repaint(); } else { animationTimer.stop(); diff --git a/src/main/java/competition/simulation/MapleSimulator.java b/src/main/java/competition/simulation/MapleSimulator.java index 7c9d0016..bbbf2604 100644 --- a/src/main/java/competition/simulation/MapleSimulator.java +++ b/src/main/java/competition/simulation/MapleSimulator.java @@ -242,8 +242,8 @@ protected void updateDriveSimulation() { var speeds = swerveDriveSimulation.getActualSpeedsFieldRelative(); aKitLog.record("RobotVelocity",speeds); - aKitLog.record("RobotVelocityMagnitude", Math.sqrt(Math.pow(speeds.vxMetersPerSecond, 2) + - Math.pow(speeds.vyMetersPerSecond, 2))); + 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/DriveHermiteSplineCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java index e90895fc..c3c01c6b 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveHermiteSplineCommand.java @@ -30,7 +30,7 @@ public class DriveHermiteSplineCommand extends BaseCommand { private Landmarks.CoralStation endingCoralStation; private double distanceToStartUsingFinalHeading = 0.33; - final private HeadingModule headingModule; + final HeadingModule headingModule; Rotation2d initialHeading; Rotation2d endingHeading;