diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b4c2fe1..494bfdc 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,58 +1,66 @@ -name: CI - -on: [push, pull_request] - -jobs: - assemble: - name: Assemble - - runs-on: ubuntu-latest - - steps: - - name: Checkout Git repository - uses: actions/checkout@v4 - - name: Setup Java - uses: actions/setup-java@v4 - with: - distribution: "temurin" - java-version: 17 - cache: "gradle" - - name: Validate Gradle wrapper - uses: gradle/actions/wrapper-validation@v4 - - name: Assemble project - run: ./gradlew assemble - style: - name: Check formatting - - runs-on: ubuntu-latest - - steps: - - name: Checkout Git repository - uses: actions/checkout@v4 - - name: Setup Java - uses: actions/setup-java@v4 - with: - distribution: "temurin" - java-version: 17 - cache: "gradle" - - name: Validate Gradle wrapper - uses: gradle/actions/wrapper-validation@v4 - - name: Check formatting - run: ./gradlew spotlessCheck - style-python: - name: Check Python formatting - - runs-on: ubuntu-latest - - steps: - - name: Checkout Git repository - uses: actions/checkout@v4 - - name: Setup Python - uses: actions/setup-python@v5 - with: - python-version: "3.12" - cache: "pip" - - name: Install Python dependencies - run: pip install black - - name: Check formatting - run: black --check . +name: CI + +on: [push, pull_request] + +jobs: + validation: + name: Validate Gradle wrappers + runs-on: ubuntu-latest + steps: + - name: Checkout Git repository + uses: actions/checkout@v4 + - name: Validate Gradle wrappers + uses: gradle/actions/wrapper-validation@v4 + assemble: + name: Assemble + + runs-on: ubuntu-latest + + steps: + - name: Checkout Git repository + uses: actions/checkout@v4 + - name: Setup Java + uses: actions/setup-java@v4 + with: + distribution: "temurin" + java-version: 17 + cache: "gradle" + - name: Validate Gradle wrapper + uses: gradle/actions/wrapper-validation@v4 + - name: Assemble project + run: ./gradlew assemble + style: + name: Check formatting + + runs-on: ubuntu-latest + + steps: + - name: Checkout Git repository + uses: actions/checkout@v4 + - name: Setup Java + uses: actions/setup-java@v4 + with: + distribution: "temurin" + java-version: 17 + cache: "gradle" + - name: Validate Gradle wrapper + uses: gradle/actions/wrapper-validation@v4 + - name: Check formatting + run: ./gradlew spotlessCheck + style-python: + name: Check Python formatting + + runs-on: ubuntu-latest + + steps: + - name: Checkout Git repository + uses: actions/checkout@v4 + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: "3.12" + cache: "pip" + - name: Install Python dependencies + run: pip install black + - name: Check formatting + run: black --check . diff --git a/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java b/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java index 17d20a5..096b262 100644 --- a/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java +++ b/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java @@ -1,113 +1,123 @@ -package frc.robot.autos.trailblazer; - -import dev.doglog.DogLog; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.autos.trailblazer.constraints.AutoConstraintCalculator; -import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions; -import frc.robot.autos.trailblazer.followers.PathFollower; -import frc.robot.autos.trailblazer.followers.PidPathFollower; -import frc.robot.autos.trailblazer.trackers.HeuristicPathTracker; -import frc.robot.autos.trailblazer.trackers.PathTracker; -import frc.robot.localization.LocalizationSubsystem; -import frc.robot.swerve.SwerveSubsystem; - -public class Trailblazer { - /** - * Given a point and the constraints for its parent segment, resolve the constraint options to use - * while following that point. - */ - private static AutoConstraintOptions resolveConstraints( - AutoPoint point, AutoConstraintOptions segmentConstraints) { - return point.constraints.orElse(segmentConstraints); - } - - private final SwerveSubsystem swerve; - private final LocalizationSubsystem localization; - private final PathTracker pathTracker = new HeuristicPathTracker(); - private final PathFollower pathFollower = - new PidPathFollower( - new PIDController(4, 0, 0), new PIDController(4, 0, 0), new PIDController(2.5, 0, 0)); - private int previousAutoPointIndex = -1; - - public Trailblazer(SwerveSubsystem swerve, LocalizationSubsystem localization) { - this.swerve = swerve; - this.localization = localization; - } - - public Command followSegment(AutoSegment segment) { - return followSegment(segment, true); - } - - public Command followSegment(AutoSegment segment, boolean shouldEnd) { - var command = - Commands.runOnce( - () -> { - pathTracker.resetAndSetPoints(segment.points); - - DogLog.log( - "Trailblazer/CurrentSegment/InitialPoints", - segment.points.stream() - .map(point -> point.poseSupplier.get()) - .toArray(Pose2d[]::new)); - }) - .alongWith( - Commands.run( - () -> { - pathTracker.updateRobotState( - localization.getPose(), swerve.getFieldRelativeSpeeds()); - - var currentAutoPointIndex = pathTracker.getCurrentPointIndex(); - var currentAutoPoint = segment.points.get(currentAutoPointIndex); - - var constrainedVelocityGoal = - getSwerveSetpoint(currentAutoPoint, segment.defaultConstraints); - swerve.setFieldRelativeAutoSpeeds(constrainedVelocityGoal); - - DogLog.log("Trailblazer/Tracker/CurrentPointIndex", currentAutoPointIndex); - if (previousAutoPointIndex != currentAutoPointIndex) { - // Currently tracked point has changed, trigger side effects - - // Each of the points in (previous, current] - var pointsToRunSideEffectsFor = - segment.points.subList( - previousAutoPointIndex + 1, currentAutoPointIndex + 1); - for (var passedPoint : pointsToRunSideEffectsFor) { - DogLog.log( - "Trailblazer/Tracker/CommandTriggered", - passedPoint.command.getName()); - passedPoint.command.schedule(); - } - previousAutoPointIndex = currentAutoPointIndex; - } - }, - swerve)) - .withName("FollowSegmentIndefinitely"); - - if (shouldEnd) { - return command.until(pathTracker::isFinished).withName("FollowSegmentUntilFinished"); - } - - return command; - } - - private ChassisSpeeds getSwerveSetpoint( - AutoPoint point, AutoConstraintOptions segmentConstraints) { - var usedConstraints = resolveConstraints(point, segmentConstraints); - - var originalTargetPose = pathTracker.getTargetPose(); - DogLog.log("Trailblazer/Tracker/RawOutput", originalTargetPose); - - var originalVelocityGoal = - pathFollower.calculateSpeeds(localization.getPose(), originalTargetPose); - DogLog.log("Trailblazer/Follower/RawOutput", originalVelocityGoal); - var constrainedVelocityGoal = - AutoConstraintCalculator.constrainVelocityGoal(originalVelocityGoal, usedConstraints); - DogLog.log("Trailblazer/Follower/UsedOutput", constrainedVelocityGoal); - - return constrainedVelocityGoal; - } -} +package frc.robot.autos.trailblazer; + +import dev.doglog.DogLog; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.autos.trailblazer.constraints.AutoConstraintCalculator; +import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions; +import frc.robot.autos.trailblazer.followers.PathFollower; +import frc.robot.autos.trailblazer.followers.PidPathFollower; +import frc.robot.autos.trailblazer.trackers.HeuristicPathTracker; +import frc.robot.autos.trailblazer.trackers.PathTracker; +import frc.robot.localization.LocalizationSubsystem; +import frc.robot.swerve.SwerveSubsystem; + +public class Trailblazer { + /** + * Given a point and the constraints for its parent segment, resolve the constraint options to use + * while following that point. + */ + private static AutoConstraintOptions resolveConstraints( + AutoPoint point, AutoConstraintOptions segmentConstraints) { + return point.constraints.orElse(segmentConstraints); + } + + private final SwerveSubsystem swerve; + private final LocalizationSubsystem localization; + private final PathTracker pathTracker = new HeuristicPathTracker(); + private final PathFollower pathFollower = + new PidPathFollower( + new PIDController(4, 0, 0), new PIDController(4, 0, 0), new PIDController(2.5, 0, 0)); + private int previousAutoPointIndex = -1; + private ChassisSpeeds previousSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + private double previousTimestamp = 0.0; + + public Trailblazer(SwerveSubsystem swerve, LocalizationSubsystem localization) { + this.swerve = swerve; + this.localization = localization; + } + + public Command followSegment(AutoSegment segment) { + return followSegment(segment, true); + } + + public Command followSegment(AutoSegment segment, boolean shouldEnd) { + var command = + Commands.runOnce( + () -> { + pathTracker.resetAndSetPoints(segment.points); + + DogLog.log( + "Trailblazer/CurrentSegment/InitialPoints", + segment.points.stream() + .map(point -> point.poseSupplier.get()) + .toArray(Pose2d[]::new)); + }) + .alongWith( + Commands.run( + () -> { + pathTracker.updateRobotState( + localization.getPose(), swerve.getFieldRelativeSpeeds()); + + var currentAutoPointIndex = pathTracker.getCurrentPointIndex(); + var currentAutoPoint = segment.points.get(currentAutoPointIndex); + + var constrainedVelocityGoal = + getSwerveSetpoint(currentAutoPoint, segment.defaultConstraints); + swerve.setFieldRelativeAutoSpeeds(constrainedVelocityGoal); + + DogLog.log("Trailblazer/Tracker/CurrentPointIndex", currentAutoPointIndex); + if (previousAutoPointIndex != currentAutoPointIndex) { + // Currently tracked point has changed, trigger side effects + + // Each of the points in (previous, current] + var pointsToRunSideEffectsFor = + segment.points.subList( + previousAutoPointIndex + 1, currentAutoPointIndex + 1); + for (var passedPoint : pointsToRunSideEffectsFor) { + DogLog.log( + "Trailblazer/Tracker/CommandTriggered", + passedPoint.command.getName()); + passedPoint.command.schedule(); + } + previousAutoPointIndex = currentAutoPointIndex; + } + }, + swerve)) + .withName("FollowSegmentIndefinitely"); + + if (shouldEnd) { + return command.until(pathTracker::isFinished).withName("FollowSegmentUntilFinished"); + } + + return command; + } + + private ChassisSpeeds getSwerveSetpoint( + AutoPoint point, AutoConstraintOptions segmentConstraints) { + double currentTimestamp = Timer.getFPGATimestamp(); + if (previousTimestamp == 0.0){ + previousTimestamp = currentTimestamp - 0.02; + } + var usedConstraints = resolveConstraints(point, segmentConstraints); + + var originalTargetPose = pathTracker.getTargetPose(); + DogLog.log("Trailblazer/Tracker/RawOutput", originalTargetPose); + + var originalVelocityGoal = + pathFollower.calculateSpeeds(localization.getPose(), originalTargetPose); + DogLog.log("Trailblazer/Follower/RawOutput", originalVelocityGoal); + var constrainedVelocityGoal = + AutoConstraintCalculator.constrainVelocityGoal(originalVelocityGoal, previousSpeeds, currentTimestamp - previousTimestamp, usedConstraints); + DogLog.log("Trailblazer/Follower/UsedOutput", constrainedVelocityGoal); + + previousTimestamp = currentTimestamp; + previousSpeeds = constrainedVelocityGoal; + + return constrainedVelocityGoal; + } +} diff --git a/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java b/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java index bd75285..7b33530 100644 --- a/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java +++ b/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java @@ -1,67 +1,70 @@ -package frc.robot.autos.trailblazer.constraints; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public class AutoConstraintCalculator { - - public static ChassisSpeeds constrainLinearVelocity( - ChassisSpeeds inputSpeeds, - AutoConstraintOptions options){ - // TODO: Implement linear velocity constraint - return inputSpeeds; - } - - public static ChassisSpeeds constrainRotationalVelocity( - ChassisSpeeds inputSpeeds, - AutoConstraintOptions options) { - // TODO: Implement rotational velocity constraint - return inputSpeeds; - } - - public static ChassisSpeeds constrainLinearAcceleration( - ChassisSpeeds inputSpeeds, - ChassisSpeeds previousSpeeds, - double timeBetweenPreviousAndInputSpeeds, - AutoConstraintOptions options){ - // TODO: Implement linear acceleration constraint - return inputSpeeds; - } - - public static ChassisSpeeds constrainRotationalAcceleration( - ChassisSpeeds inputSpeeds, - ChassisSpeeds previousSpeeds, - double timeBetweenPreviousAndInputSpeeds, - AutoConstraintOptions options){ - // TODO: Implement angular acceleration constraint - return inputSpeeds; - } - - public static ChassisSpeeds constrainVelocityGoal( - ChassisSpeeds inputSpeeds, - ChassisSpeeds previousSpeeds, - double timeBetweenPreviousAndInputSpeeds, - AutoConstraintOptions options) { - ChassisSpeeds constrainedSpeeds = inputSpeeds; - - if (options.maxLinearVelocity() != 0) { - constrainedSpeeds = constrainLinearVelocity(constrainedSpeeds, options); - } - - if (options.maxAngularVelocity() != 0) { - constrainedSpeeds = constrainRotationalVelocity(constrainedSpeeds, options); - } - - if (options.maxLinearAcceleration() != 0) { - constrainedSpeeds = constrainLinearAcceleration(constrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); - } - - if (options.maxAngularAcceleration() != 0) { - constrainedSpeeds = constrainRotationalAcceleration(constrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); - } - - return constrainedSpeeds; - } - - private AutoConstraintCalculator() {} -} +package frc.robot.autos.trailblazer.constraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class AutoConstraintCalculator { + + public static ChassisSpeeds constrainLinearVelocity( + ChassisSpeeds inputSpeeds, + AutoConstraintOptions options){ + // TODO: Implement linear velocity constraint + return inputSpeeds; + } + + public static ChassisSpeeds constrainRotationalVelocity( + ChassisSpeeds inputSpeeds, + AutoConstraintOptions options) { + // TODO: Implement rotational velocity constraint + return inputSpeeds; + } + + public static ChassisSpeeds constrainLinearAcceleration( + ChassisSpeeds inputSpeeds, + ChassisSpeeds previousSpeeds, + double timeBetweenPreviousAndInputSpeeds, + AutoConstraintOptions options){ + // TODO: Implement linear acceleration constraint + // Could approach this by seeing if the acceleration exceeds the max acceleration. + // If it does, calculate the maximum velocity to achieve the max acceleration and + // use the same velocity clamp function as above. + return inputSpeeds; + } + + public static ChassisSpeeds constrainRotationalAcceleration( + ChassisSpeeds inputSpeeds, + ChassisSpeeds previousSpeeds, + double timeBetweenPreviousAndInputSpeeds, + AutoConstraintOptions options){ + // TODO: Implement angular acceleration constraint + return inputSpeeds; + } + + public static ChassisSpeeds constrainVelocityGoal( + ChassisSpeeds inputSpeeds, + ChassisSpeeds previousSpeeds, + double timeBetweenPreviousAndInputSpeeds, + AutoConstraintOptions options) { + ChassisSpeeds constrainedSpeeds = inputSpeeds; + + if (options.maxLinearVelocity() != 0) { + constrainedSpeeds = constrainLinearVelocity(constrainedSpeeds, options); + } + + if (options.maxAngularVelocity() != 0) { + constrainedSpeeds = constrainRotationalVelocity(constrainedSpeeds, options); + } + + if (options.maxLinearAcceleration() != 0) { + constrainedSpeeds = constrainLinearAcceleration(constrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); + } + + if (options.maxAngularAcceleration() != 0) { + constrainedSpeeds = constrainRotationalAcceleration(constrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); + } + + return constrainedSpeeds; + } + + private AutoConstraintCalculator() {} +} diff --git a/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintOptions.java b/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintOptions.java index 867eca5..fc9bddb 100644 --- a/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintOptions.java +++ b/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintOptions.java @@ -1,63 +1,63 @@ -package frc.robot.autos.trailblazer.constraints; - -public record AutoConstraintOptions( - /** Whether collision avoidance should be enabled. */ - boolean collisionAvoidance, - /** Max linear velocity allowed in meters per second. Set to 0 to disable. */ - double maxLinearVelocity, - /** Max angular velocity allowed in radians per second. Set to 0 to disable. */ - double maxAngularVelocity, - /** Max linear acceleration allowed in meters per second squared. Set to 0 to disable. */ - double maxLinearAcceleration, - /** Max angular acceleration allowed in radians per second squared. Set to 0 to disable. */ - double maxAngularAcceleration) { - /** Default constraint options to use if no point or segment specific options are set. */ - public AutoConstraintOptions() { - this(false, 0, 0, 0, 0); - } - - public AutoConstraintOptions withCollisionAvoidance(boolean collisionAvoidance) { - return new AutoConstraintOptions( - collisionAvoidance, - maxLinearVelocity(), - maxAngularVelocity(), - maxLinearAcceleration(), - maxAngularAcceleration()); - } - - public AutoConstraintOptions withMaxLinearVelocity(double maxLinearVelocity) { - return new AutoConstraintOptions( - collisionAvoidance(), - maxLinearVelocity, - maxAngularVelocity(), - maxLinearAcceleration(), - maxAngularAcceleration()); - } - - public AutoConstraintOptions withMaxAngularVelocity(double maxAngularVelocity) { - return new AutoConstraintOptions( - collisionAvoidance(), - maxLinearVelocity(), - maxAngularVelocity, - maxLinearAcceleration(), - maxAngularAcceleration()); - } - - public AutoConstraintOptions withMaxLinearAcceleration(double maxLinearAcceleration) { - return new AutoConstraintOptions( - collisionAvoidance(), - maxLinearVelocity(), - maxAngularVelocity(), - maxLinearAcceleration, - maxAngularAcceleration()); - } - - public AutoConstraintOptions withMaxAngularAcceleration(double maxAngularAcceleration) { - return new AutoConstraintOptions( - collisionAvoidance(), - maxLinearVelocity(), - maxAngularVelocity(), - maxLinearAcceleration(), - maxAngularAcceleration); - } -} +package frc.robot.autos.trailblazer.constraints; + +public record AutoConstraintOptions( + /** Whether collision avoidance should be enabled. */ + boolean collisionAvoidance, + /** Max linear velocity allowed in meters per second. Set to 0 to disable. */ + double maxLinearVelocity, + /** Max angular velocity allowed in radians per second. Set to 0 to disable. */ + double maxAngularVelocity, + /** Max linear acceleration allowed in meters per second squared. Set to 0 to disable. */ + double maxLinearAcceleration, + /** Max angular acceleration allowed in radians per second squared. Set to 0 to disable. */ + double maxAngularAcceleration) { + /** Default constraint options to use if no point or segment specific options are set. */ + public AutoConstraintOptions() { + this(false, 0, 0, 0, 0); + } + + public AutoConstraintOptions withCollisionAvoidance(boolean collisionAvoidance) { + return new AutoConstraintOptions( + collisionAvoidance, + maxLinearVelocity(), + maxAngularVelocity(), + maxLinearAcceleration(), + maxAngularAcceleration()); + } + + public AutoConstraintOptions withMaxLinearVelocity(double maxLinearVelocity) { + return new AutoConstraintOptions( + collisionAvoidance(), + maxLinearVelocity, + maxAngularVelocity(), + maxLinearAcceleration(), + maxAngularAcceleration()); + } + + public AutoConstraintOptions withMaxAngularVelocity(double maxAngularVelocity) { + return new AutoConstraintOptions( + collisionAvoidance(), + maxLinearVelocity(), + maxAngularVelocity, + maxLinearAcceleration(), + maxAngularAcceleration()); + } + + public AutoConstraintOptions withMaxLinearAcceleration(double maxLinearAcceleration) { + return new AutoConstraintOptions( + collisionAvoidance(), + maxLinearVelocity(), + maxAngularVelocity(), + maxLinearAcceleration, + maxAngularAcceleration()); + } + + public AutoConstraintOptions withMaxAngularAcceleration(double maxAngularAcceleration) { + return new AutoConstraintOptions( + collisionAvoidance(), + maxLinearVelocity(), + maxAngularVelocity(), + maxLinearAcceleration(), + maxAngularAcceleration); + } +} diff --git a/src/test/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculatorTest.java b/src/test/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculatorTest.java new file mode 100644 index 0000000..3955036 --- /dev/null +++ b/src/test/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculatorTest.java @@ -0,0 +1,177 @@ +package frc.robot.autos.trailblazer.constraints; + +import frc.robot.autos.trailblazer.constraints.AutoConstraintCalculator; +import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + + +public class AutoConstraintCalculatorTest { + + private static double round(double input){ + return Math.round(input * 100.0) / 100.0; + } + + private static ChassisSpeeds round(ChassisSpeeds speeds){ + return new ChassisSpeeds( + round(speeds.vxMetersPerSecond), + round(speeds.vyMetersPerSecond), + round(speeds.omegaRadiansPerSecond) + ); + } + + private static boolean isEqual(ChassisSpeeds a, ChassisSpeeds b){ + ChassisSpeeds roundedA = round(a); + ChassisSpeeds roundedB = round(b); + boolean equals = true; + if (roundedA.vxMetersPerSecond != roundedB.vxMetersPerSecond){ + equals = false; + System.out.print("vxMetersPerSecond, "); + System.out.print(roundedA.vxMetersPerSecond + " != "); + System.out.print(roundedB.vxMetersPerSecond + "\n"); + } + if (roundedA.vyMetersPerSecond != roundedB.vyMetersPerSecond){ + equals = false; + System.out.print("vyMetersPerSecond, "); + System.out.print(roundedA.vyMetersPerSecond + " != "); + System.out.print(roundedB.vyMetersPerSecond + "\n"); + } + if (roundedA.omegaRadiansPerSecond != roundedB.omegaRadiansPerSecond){ + equals = false; + System.out.print("omegaRadiansPerSecond, "); + System.out.print(roundedA.omegaRadiansPerSecond + " != "); + System.out.print(roundedB.omegaRadiansPerSecond + "\n"); + } + return equals; + } + + + @Test + void verifyLinearVelocityConstraint() { + /** Verify that linear constraints are being applied as expected. */ + ChassisSpeeds unconstrainedSpeeds, expectedConstrainedSpeeds, outputConstrainedSpeeds; + ChassisSpeeds previousSpeeds = new ChassisSpeeds(); + AutoConstraintOptions options; + + // Constraint x velocity from 15 down to 10. + unconstrainedSpeeds = new ChassisSpeeds(15.0, 0.0, 0.0); + options = new AutoConstraintOptions().withMaxLinearVelocity(10.0); + expectedConstrainedSpeeds = new ChassisSpeeds(10.0, 0.0, 0.0); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, 0, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + + // Constrain y velocity down from -15 down to -10. + unconstrainedSpeeds = new ChassisSpeeds(0.0, -15.0, 0.0); + options = new AutoConstraintOptions().withMaxLinearVelocity(10.0); + expectedConstrainedSpeeds = new ChassisSpeeds(0.0, -15.0, 0.0); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, 0, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + + // Constrain x velocity of 8 and y velocity of 9 down to 10. + // The combined input velocity is sqrt(8**2 + 9**2), which is 12.04. + // To constrain this down to 10, we have to preserve the initial vector angle, which can be found by + // theta = arctan(y / x) = arctan(9 / 8) = 48.366461 degrees + // Applying that angle with the new clamped velocity of 10, we can find the new x and y velocity by + // x = hypotenuse * cos(theta) = 10 * cos(48.366461 degrees) = 6.64 + // y = hypotenuse * sin(theta) = 10 * sin(48.366461 degrees) = 7.47 + // Optionally, this can also be approximated by taking ratio of unconstrained total velocity over constrained velocity + // and multiplying the initial components to get the constrained components. + unconstrainedSpeeds = new ChassisSpeeds(8.0, 9.0, 0.0); + options = new AutoConstraintOptions().withMaxLinearVelocity(10.0); + expectedConstrainedSpeeds = new ChassisSpeeds(6.64, 7.47, 0.0); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, 0, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + } + + @Test + void verifyRotationalVelocityConstraints() { + /** Verify that rotational constraints are being applied as expected. */ + ChassisSpeeds unconstrainedSpeeds, expectedConstrainedSpeeds, outputConstrainedSpeeds; + ChassisSpeeds previousSpeeds = new ChassisSpeeds(); + AutoConstraintOptions options; + + // Constrain rotational velocity from 15 to 10. + unconstrainedSpeeds = new ChassisSpeeds(0, 0, 15); + options = new AutoConstraintOptions().withMaxAngularVelocity(10); + expectedConstrainedSpeeds = new ChassisSpeeds(0, 0, 10); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, 0, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + } + + @Test + void verifyLinearAccelerationConstraints() { + /** Verify that acceleration linear constraints are being applied as expected. */ + ChassisSpeeds unconstrainedSpeeds, expectedConstrainedSpeeds, outputConstrainedSpeeds, previousSpeeds; + double timeBetweenPreviousAndInputSpeeds; + AutoConstraintOptions options; + + // Constrain linear acceleration to 10. + // Input speed is 20, previous speed is 10, time between samples is half a second. + // Unconstrained acceleration = (20 - 10) / 0.5 = 20 + // Constraining the acceleration to 10 makes it so the following formula must be met + // (constrainedSpeed - 10) / 0.5 = 10 + // This makes it so constrainedSpeed = (10 * 0.5) + 10 = 15. + unconstrainedSpeeds = new ChassisSpeeds(20.0, 0, 0); + previousSpeeds = new ChassisSpeeds(10.0, 0, 0); + timeBetweenPreviousAndInputSpeeds = 0.5; // Half a second. + options = new AutoConstraintOptions().withMaxLinearAcceleration(10); + expectedConstrainedSpeeds = new ChassisSpeeds(15.0, 0, 0); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + + // Unconstrained combined velocity = sqrt(20**2 + 25**2) = 32.02 + // Previous combined velocity = sqrt(10**2 + 5**2) = 11.18 + // Total acceleration = (32.02 - 11.18) / 0.5 = 41.68. + // Clamping it, our max velocity should be = (max velocity - 11.18) / 0.5 = 10 + // max velocity = 16.18 + // Clamp this like max velocity. + // theta of travel is = arctan(y / x) = arctan(25 / 20) = 51.34 degrees + // x = hypotenuse * cos(theta) = 16.18 * cos(51.34) = 10.11 + // y = hypotenuse * sin(theta) = 16.18 * sin(51.34) = 12.63 + // OR + // x = (constrained velocity / unconstrained velocity) * unconstrained x = (16.18 / 32.02) * 20 = 10.11 + // y = (constrained velocity / unconstrained velocity) * unconstrained y = (16.18 / 32.02) * 25 = 12.63 + unconstrainedSpeeds = new ChassisSpeeds(20.0, 25.0, 0); + previousSpeeds = new ChassisSpeeds(10.0, 5.0, 0); + timeBetweenPreviousAndInputSpeeds = 0.5; // Half a second. + options = new AutoConstraintOptions().withMaxLinearAcceleration(10); + expectedConstrainedSpeeds = new ChassisSpeeds(10.11, 12.63, 0); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + } + + @Test + void verifyRotationalAccelerationConstraints() { + /** Verify that rotational accelerations constraints are being applied as expected. */ + ChassisSpeeds unconstrainedSpeeds, expectedConstrainedSpeeds, outputConstrainedSpeeds, previousSpeeds; + double timeBetweenPreviousAndInputSpeeds; + AutoConstraintOptions options; + + // Constrain rotational acceleration to 10. + // Since there are no linear constraints, preserve linear velocity. + // Angular acceleration = (unconstrained speed - previous speed) / time = (10 - -10) / 0.25 = 20 / 0.25 = 80 + // Constraining it to 10 means that + // 10 = (constrained speed - previous speed) / time + // 10 = (constrained speed - -10) / 0.25, constrained speed = 2.5 - 10 = -7.5 + // Rotational speed = -7.5 + unconstrainedSpeeds = new ChassisSpeeds(10.0, 10.0, 10.0); + previousSpeeds = new ChassisSpeeds(0.0, 0.0, -10.0); + timeBetweenPreviousAndInputSpeeds = 0.25; // Half a second. + options = new AutoConstraintOptions().withMaxAngularAcceleration(10.0); + expectedConstrainedSpeeds = new ChassisSpeeds(10.0, 10.0, -7.5); + outputConstrainedSpeeds = AutoConstraintCalculator.constrainVelocityGoal(unconstrainedSpeeds, previousSpeeds, timeBetweenPreviousAndInputSpeeds, options); + Assertions.assertTrue(isEqual(outputConstrainedSpeeds, expectedConstrainedSpeeds)); + } + + @Test + void verifyAllConstraintsAtOnce() { + /** Verify that all constraints being applied on a complex problem at once works. */ + // TODO: Craft a test that tests all constraints being applied and used. + } + +} +