code for limiting the accleration of a swerve drive

Drive.java

    @NotNull ChassisSpeeds lastRequestedVelocity = new ChassisSpeeds(0, 0, 0);

    private double lastLoopTime = 0;

    /**
     * Puts a limit on the acceleration. This method should be called before setting a chassis speeds to the robot drivebase.
     * <p>
     * Limits the acceleration by ensuring that the difference between the command and previous velocity doesn't exceed a 
     * set acceleration * delata time since the last calculation
     *
     * @param commandedVelocity Desired Field Relative Chassis Speeds (The chassis speeds is mutated to the limited acceleration)
     * @return an acceleration limited chassis speeds
     */
    @NotNull ChassisSpeeds limitAcceleration(@NotNull ChassisSpeeds commandedVelocity) {
        double dt;
        if ((Timer.getFPGATimestamp() - lastLoopTime) > ((double) Constants.DRIVE_PERIOD / 1000) * 4) {
            // If the dt is a lot greater than our nominal dt reset the acceleration limiting
            // (ex. we've been disabled for a while)
            lastRequestedVelocity = RobotTracker.getInstance().getLatencyCompedChassisSpeeds();
            dt = (double) Constants.DRIVE_PERIOD / 1000;
        } else {
            dt = Timer.getFPGATimestamp() - lastLoopTime;
        }
        lastLoopTime = Timer.getFPGATimestamp();

        double maxVelocityChange = Constants.MAX_ACCELERATION * dt;
        double maxAngularVelocityChange = Constants.MAX_ANGULAR_ACCELERATION * dt;

        Translation2d velocityCommand = new Translation2d(
                commandedVelocity.vxMetersPerSecond, commandedVelocity.vyMetersPerSecond
        );

        Translation2d lastVelocityCommand = new Translation2d(
                lastRequestedVelocity.vxMetersPerSecond, lastRequestedVelocity.vyMetersPerSecond
        );

        Translation2d velocityChange = velocityCommand.minus(lastVelocityCommand);
        double velocityChangeAngle = Math.atan2(velocityChange.getY(), velocityChange.getX()); //Radians

        // Check if velocity change exceeds max limit
        if (velocityChange.getNorm() > maxVelocityChange) {
            // Get limited velocity vector difference in cartesian coordinate system
            Translation2d limitedVelocityVectorChange = new Translation2d(maxVelocityChange, new Rotation2d(velocityChangeAngle));
            Translation2d limitedVelocityVector = lastVelocityCommand.plus(limitedVelocityVectorChange);

            commandedVelocity.vyMetersPerSecond = limitedVelocityVector.getX();
            commandedVelocity.vyMetersPerSecond = limitedVelocityVector.getY();
        }

        // Checks if requested change in Angular Velocity is greater than allowed
        if (Math.abs(commandedVelocity.omegaRadiansPerSecond - lastRequestedVelocity.omegaRadiansPerSecond)
                > maxAngularVelocityChange) {
            // Add the lastCommandVelocity and the maxAngularVelocityChange (changed to have the same sign as the actual change)
            commandedVelocity.omegaRadiansPerSecond =
                    lastRequestedVelocity.omegaRadiansPerSecond +
                            Math.copySign(maxAngularVelocityChange,
                                    commandedVelocity.omegaRadiansPerSecond - lastRequestedVelocity.omegaRadiansPerSecond);
        }

        lastRequestedVelocity = commandedVelocity; // save our current commanded velocity to be used in next iteration
        return commandedVelocity;
    }