diff --git a/build.gradle b/build.gradle index 6148d2fc..d16f84fb 100644 --- a/build.gradle +++ b/build.gradle @@ -136,7 +136,7 @@ spotless { exclude '**/build/**', '**/build-*/**' } toggleOffOn() - googleJavaFormat() + googleJavaFormat().aosp() trimTrailingWhitespace() endWithNewline() } diff --git a/src/main/deploy/pathplanner/paths/1M.path b/src/main/deploy/pathplanner/paths/1M.path index fec249cd..04db582d 100644 --- a/src/main/deploy/pathplanner/paths/1M.path +++ b/src/main/deploy/pathplanner/paths/1M.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": -0.27527965608765825, - "y": 0.0 + "y": 1.561767148845488e-16 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Half-Field Sprint.path b/src/main/deploy/pathplanner/paths/Half-Field Sprint.path index 5d078552..234946c2 100644 --- a/src/main/deploy/pathplanner/paths/Half-Field Sprint.path +++ b/src/main/deploy/pathplanner/paths/Half-Field Sprint.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, + "maxVelocity": 4.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 17293b32..35a007c5 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,23 +9,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, - "robotMOI": 6.883, + "robotMass": 68.0389, + "robotMOI": 11.61, "robotTrackwidth": 0.508, "driveWheelRadius": 0.0508, - "driveGearing": 6.75, - "maxDriveSpeed": 4.572, + "driveGearing": 5.142857, + "maxDriveSpeed": 6.21, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 85.0, "wheelCOF": 1.0, - "flModuleX": 0.298, - "flModuleY": 0.298, - "frModuleX": 0.298, - "frModuleY": -0.298, - "blModuleX": -0.298, - "blModuleY": 0.298, - "brModuleX": -0.298, - "brModuleY": -0.298, + "flModuleX": 0.2921, + "flModuleY": 0.292, + "frModuleX": 0.2921, + "frModuleY": -0.2921, + "blModuleX": -0.2921, + "blModuleY": 0.2921, + "brModuleX": -0.2921, + "brModuleY": -0.2921, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java new file mode 100644 index 00000000..1234d564 --- /dev/null +++ b/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java @@ -0,0 +1,645 @@ +package com.pathplanner.lib.util.swerve; + +import static edu.wpi.first.units.Units.*; + +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.util.DriveFeedforwards; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +/** + * Swerve setpoint generator based on a version created by FRC team 254. + * + *

Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the + * kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any + * forces acting on a module's wheel from exceeding the force of friction. + */ +public class SwerveSetpointGenerator { + private static final double kEpsilon = 1E-8; + private static final int MAX_STEER_ITERATIONS = 8; + private static final int MAX_DRIVE_ITERATIONS = 10; + + private final RobotConfig config; + private final double maxSteerVelocityRadsPerSec; + private final double brownoutVoltage; + + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocityRadsPerSec The maximum rotation velocity of a swerve module, in + * radians per second + */ + public SwerveSetpointGenerator(RobotConfig config, double maxSteerVelocityRadsPerSec) { + this.config = config; + this.maxSteerVelocityRadsPerSec = maxSteerVelocityRadsPerSec; + this.brownoutVoltage = RobotController.getBrownoutVoltage(); + } + + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocity The maximum rotation velocity of a swerve module + */ + public SwerveSetpointGenerator(RobotConfig config, AngularVelocity maxSteerVelocity) { + this(config, maxSteerVelocity.in(RadiansPerSecond)); + } + + /** + * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds + * passed into or returned from this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also + * be a static nominal voltage if you do not want the setpoint generator to react to changes + * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input + * voltage will be clamped to a minimum of the robot controller's brownout voltage. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredStateRobotRelative, + double dt, + double inputVoltage) { + if (Double.isNaN(inputVoltage)) { + inputVoltage = 12.0; + } else { + inputVoltage = Math.max(inputVoltage, brownoutVoltage); + } + + SwerveModuleState[] desiredModuleStates = + config.toSwerveModuleStates(desiredStateRobotRelative); + // Make sure desiredState respects velocity limits. + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredModuleStates, config.moduleConfig.maxDriveVelocityMPS); + desiredStateRobotRelative = config.toChassisSpeeds(desiredModuleStates); + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, + // so + // just use the previous angle. + boolean need_to_steer = true; + if (epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { + need_to_steer = false; + for (int m = 0; m < config.numModules; m++) { + desiredModuleStates[m].angle = prevSetpoint.moduleStates()[m].angle; + desiredModuleStates[m].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[config.numModules]; + double[] prev_vy = new double[config.numModules]; + Rotation2d[] prev_heading = new Rotation2d[config.numModules]; + double[] desired_vx = new double[config.numModules]; + double[] desired_vy = new double[config.numModules]; + Rotation2d[] desired_heading = new Rotation2d[config.numModules]; + boolean all_modules_should_flip = true; + for (int m = 0; m < config.numModules; m++) { + prev_vx[m] = + prevSetpoint.moduleStates()[m].angle.getCos() + * prevSetpoint.moduleStates()[m].speedMetersPerSecond; + prev_vy[m] = + prevSetpoint.moduleStates()[m].angle.getSin() + * prevSetpoint.moduleStates()[m].speedMetersPerSecond; + prev_heading[m] = prevSetpoint.moduleStates()[m].angle; + if (prevSetpoint.moduleStates()[m].speedMetersPerSecond < 0.0) { + prev_heading[m] = prev_heading[m].rotateBy(Rotation2d.k180deg); + } + desired_vx[m] = + desiredModuleStates[m].angle.getCos() + * desiredModuleStates[m].speedMetersPerSecond; + desired_vy[m] = + desiredModuleStates[m].angle.getSin() + * desiredModuleStates[m].speedMetersPerSecond; + desired_heading[m] = desiredModuleStates[m].angle; + if (desiredModuleStates[m].speedMetersPerSecond < 0.0) { + desired_heading[m] = desired_heading[m].rotateBy(Rotation2d.k180deg); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs( + prev_heading[m] + .unaryMinus() + .rotateBy(desired_heading[m]) + .getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !epsilonEquals(prevSetpoint.robotRelativeSpeeds(), new ChassisSpeeds()) + && !epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // of the desired angle, and accelerate again. + return generateSetpoint(prevSetpoint, new ChassisSpeeds(), dt, inputVoltage); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state + // to + // the goal state; then find the amount we can move from start towards goal in this cycle + // such + // that no kinematic limit is exceeded. + double dx = + desiredStateRobotRelative.vxMetersPerSecond + - prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; + double dy = + desiredStateRobotRelative.vyMetersPerSecond + - prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; + double dtheta = + desiredStateRobotRelative.omegaRadiansPerSecond + - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering + // angle + // to command (since inverse kinematics doesn't care about angle, we can be + // opportunistically + // lazy). + List> overrideSteering = new ArrayList<>(config.numModules); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle + // at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + for (int m = 0; m < config.numModules; m++) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[m].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + + double max_theta_step = dt * maxSteerVelocityRadsPerSec; + + if (epsilonEquals(prevSetpoint.moduleStates()[m].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final + // steering + // angle, so limit based purely on rotation in place. + if (epsilonEquals(desiredModuleStates[m].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(m, Optional.of(prevSetpoint.moduleStates()[m].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint + .moduleStates()[m] + .angle + .unaryMinus() + .rotateBy(desiredModuleStates[m].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.kPi); + } + + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = + Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(m, Optional.of(desiredModuleStates[m].angle)); + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + m, + Optional.of( + prevSetpoint.moduleStates()[m].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) + * max_theta_step)))); + min_s = 0.0; + } + continue; + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + // Enforce centripetal force limits to prevent sliding. + // We do this by changing max_theta_step to the maximum change in heading over dt + // that would create a large enough radius to keep the centripetal force under the + // friction force. + double maxHeadingChange = + (dt * config.wheelFrictionForce) + / ((config.massKG / config.numModules) + * Math.abs( + prevSetpoint.moduleStates()[m].speedMetersPerSecond)); + max_theta_step = Math.min(max_theta_step, maxHeadingChange); + + double s = + findSteeringMaxS( + prev_vx[m], + prev_vy[m], + prev_heading[m].getRadians(), + desired_vx[m], + desired_vy[m], + desired_heading[m].getRadians(), + max_theta_step); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel torque limits + Translation2d chassisForceVec = new Translation2d(); + double chassisTorque = 0.0; + for (int m = 0; m < config.numModules; m++) { + double lastVelRadPerSec = + prevSetpoint.moduleStates()[m].speedMetersPerSecond + / config.moduleConfig.wheelRadiusMeters; + // Use the current battery voltage since we won't be able to supply 12v if the + // battery is sagging down to 11v, which will affect the max torque output + double currentDraw = + config.moduleConfig.driveMotor.getCurrent( + Math.abs(lastVelRadPerSec), inputVoltage); + double reverseCurrentDraw = + Math.abs( + config.moduleConfig.driveMotor.getCurrent( + Math.abs(lastVelRadPerSec), -inputVoltage)); + currentDraw = Math.min(currentDraw, config.moduleConfig.driveCurrentLimit); + reverseCurrentDraw = + Math.min(reverseCurrentDraw, config.moduleConfig.driveCurrentLimit); + double forwardModuleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw); + double reverseModuleTorque = + config.moduleConfig.driveMotor.getTorque(reverseCurrentDraw); + + double prevSpeed = prevSetpoint.moduleStates()[m].speedMetersPerSecond; + desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); + double desiredSpeed = desiredModuleStates[m].speedMetersPerSecond; + + int forceSign; + Rotation2d forceAngle = prevSetpoint.moduleStates()[m].angle; + double moduleTorque; + if (epsilonEquals(prevSpeed, 0.0) + || (prevSpeed > 0 && desiredSpeed >= prevSpeed) + || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + moduleTorque = forwardModuleTorque; + // Torque loss will be fighting motor + moduleTorque -= config.moduleConfig.torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } else { + moduleTorque = reverseModuleTorque; + // Torque loss will be helping the motor + moduleTorque += config.moduleConfig.torqueLoss; + forceSign = -1; // Force will be applied in opposite direction of module + if (prevSpeed > 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = Math.min(moduleTorque, config.maxTorqueFriction); + + double forceAtCarpet = moduleTorque / config.moduleConfig.wheelRadiusMeters; + Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec.plus(moduleForceVec); + + // Calculate the torque this module will apply to the chassis + if (!epsilonEquals(0, moduleForceVec.getNorm())) { + Rotation2d angleToModule = config.moduleLocations[m].getAngle(); + Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); + chassisTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin(); + } + } + + Translation2d chassisAccelVec = chassisForceVec.div(config.massKG); + double chassisAngularAccel = chassisTorque / config.MOI; + + // Use kinematics to convert chassis accelerations to module accelerations + ChassisSpeeds chassisAccel = + new ChassisSpeeds( + chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); + var accelStates = config.toSwerveModuleStates(chassisAccel); + + for (int m = 0; m < config.numModules; m++) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + + double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * dt); + + double vx_min_s = + min_s == 1.0 + ? desired_vx[m] + : (desired_vx[m] - prev_vx[m]) * min_s + prev_vx[m]; + double vy_min_s = + min_s == 1.0 + ? desired_vy[m] + : (desired_vy[m] - prev_vy[m]) * min_s + prev_vy[m]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, + // because we + // already know we can't go faster than that. + double s = + findDriveMaxS( + prev_vx[m], + prev_vy[m], + Math.hypot(prev_vx[m], prev_vy[m]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + maxVelStep); + min_s = Math.min(min_s, s); + } + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta); + retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); + + double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; + double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; + double chassisAccelX = (retSpeeds.vxMetersPerSecond - prevVelX) / dt; + double chassisAccelY = (retSpeeds.vyMetersPerSecond - prevVelY) / dt; + double chassisForceX = chassisAccelX * config.massKG; + double chassisForceY = chassisAccelY * config.massKG; + + double angularAccel = + (retSpeeds.omegaRadiansPerSecond + - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond) + / dt; + double angTorque = angularAccel * config.MOI; + ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque); + + Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces); + + var retStates = config.toSwerveModuleStates(retSpeeds); + double[] accelFF = new double[config.numModules]; + double[] linearForceFF = new double[config.numModules]; + double[] torqueCurrentFF = new double[config.numModules]; + double[] forceXFF = new double[config.numModules]; + double[] forceYFF = new double[config.numModules]; + for (int m = 0; m < config.numModules; m++) { + double wheelForceDist = wheelForces[m].getNorm(); + double appliedForce = + wheelForceDist > 1e-6 + ? wheelForceDist + * wheelForces[m].getAngle().minus(retStates[m].angle).getCos() + : 0.0; + double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters; + double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque); + + final var maybeOverride = overrideSteering.get(m); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[m].angle.unaryMinus().rotateBy(override))) { + retStates[m].speedMetersPerSecond *= -1.0; + appliedForce *= -1.0; + torqueCurrent *= -1.0; + } + retStates[m].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[m].angle.unaryMinus().rotateBy(retStates[m].angle); + if (flipHeading(deltaRotation)) { + retStates[m].angle = retStates[m].angle.rotateBy(Rotation2d.k180deg); + retStates[m].speedMetersPerSecond *= -1.0; + appliedForce *= -1.0; + torqueCurrent *= -1.0; + } + + accelFF[m] = + (retStates[m].speedMetersPerSecond + - prevSetpoint.moduleStates()[m].speedMetersPerSecond) + / dt; + linearForceFF[m] = appliedForce; + torqueCurrentFF[m] = torqueCurrent; + forceXFF[m] = wheelForces[m].getX(); + forceYFF[m] = wheelForces[m].getY(); + } + + return new SwerveSetpoint( + retSpeeds, + retStates, + new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF)); + } + + /** + * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds + * passed into or returned from this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also + * be a static nominal voltage if you do not want the setpoint generator to react to changes + * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input + * voltage will be clamped to a minimum of the robot controller's brownout voltage. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredStateRobotRelative, + Time dt, + Voltage inputVoltage) { + return generateSetpoint( + prevSetpoint, desiredStateRobotRelative, dt.in(Seconds), inputVoltage.in(Volts)); + } + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + *

Note: This method will automatically use the current robot controller input voltage. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, double dt) { + return generateSetpoint( + prevSetpoint, desiredStateRobotRelative, dt, RobotController.getInputVoltage()); + } + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + *

Note: This method will automatically use the current robot controller input voltage. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, Time dt) { + return generateSetpoint( + prevSetpoint, + desiredStateRobotRelative, + dt.in(Seconds), + RobotController.getBatteryVoltage()); + } + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private static boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private static double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private static double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return s_guess; + } + + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot( + func, + x_guess, + y_guess, + f_guess, + x_1, + y_1, + f_1, + iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + private static double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = (x, y) -> unwrapAngle(f_0, Math.atan2(y, x)) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_STEER_ITERATIONS); + } + + private static double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = (x, y) -> Math.hypot(x, y) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); + } + + private static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + private static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + private static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { + return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) + && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) + && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ca46494..59504d90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,14 +15,18 @@ import frc.lib.controller.LogitechController; import frc.lib.controller.ThrustmasterJoystick; import frc.robot.constants.GlobalConstants; -import frc.robot.constants.TunerConstants; import frc.robot.constants.GlobalConstants.ControllerConstants; +import frc.robot.constants.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.WheelRadiusCharacterization; public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private double MaxSpeed = + GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = + GlobalConstants.MAX_ROTATIONAL_SPEED.in( + RadiansPerSecond); // kMaxAngularRate desired top rotational speed private final ThrustmasterJoystick leftDriveController = new ThrustmasterJoystick(ControllerConstants.LEFT_DRIVE_CONTROLLER); @@ -45,48 +49,94 @@ public RobotContainer() { configureBindings(); drivetrain.setUpPathPlanner(); - // Establish the "Trajectory Field" Field2d into the dashboard + // Establish the "Trajectory Field" Field2d into the dashboard } private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest( - () -> { - ChassisSpeeds driverDesiredSpeeds = new ChassisSpeeds( - GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) * sps(deadband(leftDriveController.getYAxis().get(), 0.1)), - sps(deadband(leftDriveController.getXAxis().get(),0.1)) * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond), - -sps(deadband(rightDriveController.getXAxis().get(),0.1)) * GlobalConstants.MAX_ROTATIONAL_SPEED.in(RadiansPerSecond) - ); - return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(driverDesiredSpeeds); - // return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds); - } - ) - ); - - // drive.withVelocityX(-leftDriveController.getYAxis().get() * GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward) - // .withVelocityY(-leftDriveController.getXAxis().get() * GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left) - // .withRotationalRate(-rightDriveController.getXAxis().get() * GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left) + // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> { + ChassisSpeeds driverDesiredSpeeds = + new ChassisSpeeds( + GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond) + * sps( + deadband( + leftDriveController + .getYAxis() + .get(), + 0.1)), + sps(deadband(leftDriveController.getXAxis().get(), 0.1)) + * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond), + -sps( + deadband( + rightDriveController + .getXAxis() + .get(), + 0.1)) + * GlobalConstants.MAX_ROTATIONAL_SPEED.in( + RadiansPerSecond)); + return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds( + driverDesiredSpeeds); + // return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds); + })); + + // drive.withVelocityX(-leftDriveController.getYAxis().get() * + // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward) + // .withVelocityY(-leftDriveController.getXAxis().get() * + // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left) + // .withRotationalRate(-rightDriveController.getXAxis().get() * + // GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left) operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake)); - operatorController.getB().whileTrue(drivetrain.applyRequest(() -> - point.withModuleDirection(new Rotation2d(-operatorController.getLeftYAxis().get(), -operatorController.getLeftXAxis().get())) - )); - - leftDriveController.getTrigger().whileTrue(new WheelRadiusCharacterization(WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain)); + operatorController + .getB() + .whileTrue( + drivetrain.applyRequest( + () -> + point.withModuleDirection( + new Rotation2d( + -operatorController.getLeftYAxis().get(), + -operatorController + .getLeftXAxis() + .get())))); + + leftDriveController + .getTrigger() + .whileTrue( + new WheelRadiusCharacterization( + WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain)); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. - operatorController.getBack().and(operatorController.getY()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); - operatorController.getBack().and(operatorController.getX()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - operatorController.getStart().and(operatorController.getY()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - operatorController.getStart().and(operatorController.getX()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + operatorController + .getBack() + .and(operatorController.getY()) + .whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + operatorController + .getBack() + .and(operatorController.getX()) + .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + operatorController + .getStart() + .and(operatorController.getY()) + .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + operatorController + .getStart() + .and(operatorController.getX()) + .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); // reset the field-centric heading on left bumper press - operatorController.getLeftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - operatorController.getRightBumper().onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); + operatorController + .getLeftBumper() + .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + operatorController + .getRightBumper() + .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); drivetrain.registerTelemetry(logger::telemeterize); } @@ -106,5 +156,4 @@ private double sps(double value) { public Command getAutonomousCommand() { return auto.getAuto(); } - } diff --git a/src/main/java/frc/robot/constants/GlobalConstants.java b/src/main/java/frc/robot/constants/GlobalConstants.java index ad4f83a6..efd9e523 100644 --- a/src/main/java/frc/robot/constants/GlobalConstants.java +++ b/src/main/java/frc/robot/constants/GlobalConstants.java @@ -5,7 +5,6 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; - import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; @@ -14,19 +13,21 @@ import edu.wpi.first.units.measure.MomentOfInertia; public class GlobalConstants { - + private static final SwerveModuleConstants EXAMPLE_MODULE = TunerConstants.FrontLeft; public static final LinearVelocity MAX_TRANSLATIONAL_SPEED = TunerConstants.kSpeedAt12Volts; - public static final AngularVelocity MAX_ROTATIONAL_SPEED = RotationsPerSecond.of(1); + public static final AngularVelocity MAX_ROTATIONAL_SPEED = RotationsPerSecond.of(1); public static final Mass ROBOT_MASS = Pounds.of(150); - public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(6.883); + public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(11.61); public static final double COEFFICIENT_OF_FRICTION = 1.0; - public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.DriveMotorGearRatio); - public static final DCMotor STEER_MOTOR = DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.SteerMotorGearRatio); + public static final DCMotor DRIVE_MOTOR = + DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.DriveMotorGearRatio); + public static final DCMotor STEER_MOTOR = + DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.SteerMotorGearRatio); public static class ControllerConstants { public static final int LEFT_DRIVE_CONTROLLER = 0; @@ -38,31 +39,52 @@ public static class ControllerConstants { public static RobotConfig getRobotConfigPathplanner() { if (robotConfigPathplanner == null) { - try{ - Translation2d[] moduleOffsets = new Translation2d[4]; - SwerveModuleConstants[] constants = new SwerveModuleConstants[]{ - TunerConstants.FrontLeft, TunerConstants.FrontRight, TunerConstants.BackLeft, TunerConstants.BackRight - }; - for (int i = 0; i < constants.length; i++) { - moduleOffsets[i] = new Translation2d(constants[i].LocationX, constants[i].LocationY); - } + try { + Translation2d[] moduleOffsets = new Translation2d[4]; + SwerveModuleConstants[] constants = + new SwerveModuleConstants[] { + TunerConstants.FrontLeft, + TunerConstants.FrontRight, + TunerConstants.BackLeft, + TunerConstants.BackRight + }; + for (int i = 0; i < constants.length; i++) { + moduleOffsets[i] = + new Translation2d(constants[i].LocationX, constants[i].LocationY); + } - robotConfigPathplanner = new RobotConfig(ROBOT_MASS, ROBOT_MOI, - new ModuleConfig( - Meters.of(EXAMPLE_MODULE.WheelRadius), - MAX_TRANSLATIONAL_SPEED, - COEFFICIENT_OF_FRICTION, - DRIVE_MOTOR, - EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits.getStatorCurrentLimitMeasure(), - 1 - ), - moduleOffsets); + robotConfigPathplanner = + new RobotConfig( + ROBOT_MASS, + ROBOT_MOI, + new ModuleConfig( + Meters.of(EXAMPLE_MODULE.WheelRadius), + MAX_TRANSLATIONAL_SPEED, + COEFFICIENT_OF_FRICTION, + DRIVE_MOTOR, + EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits + .getStatorCurrentLimitMeasure(), + 1), + moduleOffsets); } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - throw new RuntimeException("Failed to load robot config from pathplanner."); + // Handle exception as needed + e.printStackTrace(); + throw new RuntimeException("Failed to load robot config from pathplanner."); } } return robotConfigPathplanner; } + + // public static RobotConfig getRobotConfigPathplanner() { + // if (robotConfigPathplanner == null) { + // try{ + // robotConfigPathplanner = RobotConfig.fromGUISettings(); + // } catch (Exception e) { + // // Handle exception as needed + // e.printStackTrace(); + // throw new RuntimeException("Failed to load robot config from pathplanner."); + // } + // } + // return robotConfigPathplanner; + // } } diff --git a/src/main/java/frc/robot/constants/TunerConstants.java b/src/main/java/frc/robot/constants/TunerConstants.java index fe1f2401..fafebfd7 100644 --- a/src/main/java/frc/robot/constants/TunerConstants.java +++ b/src/main/java/frc/robot/constants/TunerConstants.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; - import edu.wpi.first.units.measure.*; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -25,15 +24,23 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(40).withKI(0).withKD(0.0) - .withKS(0.00).withKV(0).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(40) + .withKI(0) + .withKD(0.0) + .withKS(0.00) + .withKV(0) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + // private static final Slot0Configs driveGains = new Slot0Configs() + // .withKP(0.1).withKI(0).withKD(0) + // .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -53,14 +60,16 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can + // set a relatively low + // stator current limit to help avoid brownouts without + // impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; @@ -88,36 +97,37 @@ public class TunerConstants { // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.0439); // Simulated voltage necessary to overcome friction private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withCANcoderInitialConfigs(cancoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withCANcoderInitialConfigs(cancoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left private static final int kFrontLeftDriveMotorId = 1; @@ -163,27 +173,57 @@ public class TunerConstants { private static final Distance kBackRightXPos = Inches.of(-11.5); private static final Distance kBackRightYPos = Inches.of(-11.5); - - public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftCANcoderInverted); - public static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightCANcoderInverted); - public static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftCANcoderInverted); - public static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightCANcoderInverted); + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftCANcoderInverted); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightCANcoderInverted); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftCANcoderInverted); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightCANcoderInverted); /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. */ public static CommandSwerveDrivetrain createDrivetrain() { return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index e0c20cbb..26b73205 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -2,30 +2,19 @@ import static edu.wpi.first.units.Units.*; -import java.lang.annotation.Inherited; -import java.util.Optional; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -40,12 +29,13 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.RobotContainer; import frc.robot.constants.GlobalConstants; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; /** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily + * be used in command-based projects. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.005; // 5 ms @@ -60,78 +50,84 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private boolean m_hasAppliedOperatorPerspective = false; /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - private final SwerveRequest.ApplyRobotSpeeds m_applyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDesaturateWheelSpeeds(false); - - public final SwerveRequest.ApplyFieldSpeeds m_applyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds(); + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + + private final SwerveRequest.ApplyRobotSpeeds m_applyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDesaturateWheelSpeeds(false); + + public final SwerveRequest.ApplyFieldSpeeds m_applyFieldSpeeds = + new SwerveRequest.ApplyFieldSpeeds(); public final FieldOrientedOrbitSwerveRequest m_applyFieldSpeedsOrbit; RobotConfig config; // PathPlanner robot configuration /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); + private final SysIdRoutine m_sysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> + SignalLogger.writeString( + "SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this)); /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); + private final SysIdRoutine m_sysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> + SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this)); /* * SysId routine for characterizing rotation. * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); + private final SysIdRoutine m_sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> + SignalLogger.writeString( + "SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl( + m_rotationCharacterization.withRotationalRate( + output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this)); /* The SysId routine to test */ private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; @@ -140,10 +136,7 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private SwerveSetpointGenerator setpointGenerator; private SwerveSetpoint previousSetpoint; - public void setUpPathPlanner() { - - - } + public void setUpPathPlanner() {} public Pose2d getRobotPose() { return this.getState().Pose; @@ -155,15 +148,15 @@ public ChassisSpeeds getChassisSpeeds() { /** * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. * * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module + * @param modules Constants for each specific module */ - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { super(drivetrainConstants, modules); if (Utils.isSimulation()) { startSimThread(); @@ -173,18 +166,19 @@ public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, Sw /** * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module */ - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { super(drivetrainConstants, OdometryUpdateFrequency, modules); if (Utils.isSimulation()) { startSimThread(); @@ -195,24 +189,29 @@ public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, do /** * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * @param visionStandardDeviation The standard deviation for vision calculation - * @param modules Constants for each specific module + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * @param visionStandardDeviation The standard deviation for vision calculation + * @param modules Constants for each specific module */ public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, - Matrix odometryStandardDeviation, Matrix visionStandardDeviation, + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); if (Utils.isSimulation()) { startSimThread(); } @@ -220,47 +219,55 @@ public CommandSwerveDrivetrain( m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); } - private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() - { + private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() { RobotConfig config = GlobalConstants.getRobotConfigPathplanner(); - setpointGenerator = new SwerveSetpointGenerator( - config, - Units.rotationsToRadians(10.0) //max rotational speed - ); + setpointGenerator = + new SwerveSetpointGenerator( + config, Units.rotationsToRadians(10.0) // max rotational speed + ); - ChassisSpeeds currentSpeeds = getState().Speeds; - SwerveModuleState[] currentStates = getState().ModuleStates; - SwerveSetpoint previousSetpoint = new SwerveSetpoint(currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); + ChassisSpeeds currentSpeeds = getState().Speeds; + SwerveModuleState[] currentStates = getState().ModuleStates; + SwerveSetpoint previousSetpoint = + new SwerveSetpoint( + currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); - var request = new FieldOrientedOrbitSwerveRequest(setpointGenerator, previousSetpoint, getState().Pose.getRotation()); + var request = + new FieldOrientedOrbitSwerveRequest( + setpointGenerator, previousSetpoint, getState().Pose.getRotation()); request.withDriverOrientation(true); return request; } // - //The desired robot-relative speeds - //returns the module states where robot can drive while obeying physics and not slipping + // The desired robot-relative speeds + // returns the module states where robot can drive while obeying physics and not slipping public SwerveRequest driveRobotRelative(ChassisSpeeds speeds) { // Note: it is important to not discretize speeds before or after // using the setpoint generator, as it will discretize them for you - previousSetpoint = setpointGenerator.generateSetpoint( - previousSetpoint, // The previous setpoint - speeds, // The desired target speeds - 0.02 // The loop time of the robot code, in seconds - ); - return m_applyRobotSpeeds.withSpeeds(previousSetpoint.robotRelativeSpeeds()) - .withWheelForceFeedforwardsX(previousSetpoint.feedforwards().robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(previousSetpoint.feedforwards().robotRelativeForcesYNewtons()); - // Method that will drive the robot given target module states + previousSetpoint = + setpointGenerator.generateSetpoint( + previousSetpoint, // The previous setpoint + speeds, // The desired target speeds + 0.02 // The loop time of the robot code, in seconds + ); + return m_applyRobotSpeeds + .withSpeeds(previousSetpoint.robotRelativeSpeeds()) + .withWheelForceFeedforwardsX( + previousSetpoint.feedforwards().robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY( + previousSetpoint.feedforwards().robotRelativeForcesYNewtons()); + // Method that will drive the robot given target module states } - public SwerveRequest driveRobotRelative(double xVelocity, double yVelocity, double rotationRate) { + public SwerveRequest driveRobotRelative( + double xVelocity, double yVelocity, double rotationRate) { // Note: it is important to not discretize speeds before or after // using the setpoint generator, as it will discretize them for you ChassisSpeeds speeds = new ChassisSpeeds(xVelocity, yVelocity, rotationRate); return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds); - // Method that will drive the robot given target module states + // Method that will drive the robot given target module states } public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { @@ -268,10 +275,12 @@ public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { return driveRobotRelative(speeds); } - public SwerveRequest driveWithFeedforwards(ChassisSpeeds speeds, DriveFeedforwards feedforwards) { - return m_applyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); + public SwerveRequest driveWithFeedforwards( + ChassisSpeeds speeds, DriveFeedforwards feedforwards) { + return m_applyRobotSpeeds + .withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); } /** @@ -285,8 +294,8 @@ public Command applyRequest(Supplier requestSupplier) { } /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. + * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. * * @param direction Direction of the SysId Quasistatic test * @return Command to run @@ -296,8 +305,8 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { } /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. + * Runs the SysId Dynamic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. * * @param direction Direction of the SysId Dynamic test * @return Command to run @@ -306,9 +315,11 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.dynamic(direction); } + private double lastSpeed = 0; + @Override public void periodic() { - + /* * Periodically try to apply the operator perspective. * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. @@ -317,34 +328,60 @@ public void periodic() { * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. */ if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); + DriverStation.getAlliance() + .ifPresent( + allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); } - Logger.recordOutput("Drive/desiredChassisSpeeds", m_applyFieldSpeedsOrbit.getChassisSpeeds()); - Logger.recordOutput("Drive/setpointChassisSpeeds", m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); - - Logger.recordOutput("Drive/desiredWheelSpeeds", getState().Speeds); + Logger.recordOutput( + "Drive/desiredChassisSpeeds", m_applyFieldSpeedsOrbit.getChassisSpeeds()); + Logger.recordOutput( + "Drive/slewedChassisSpeeds", m_applyFieldSpeedsOrbit.getSlewedFieldChassisSpeeds()); + Logger.recordOutput( + "Drive/setpointChassisSpeeds", + m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); + + ChassisSpeeds speedsPreview = + m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds(); + + double currentSpeed = + Math.hypot(speedsPreview.vxMetersPerSecond, speedsPreview.vyMetersPerSecond); + double acceleration = (currentSpeed - lastSpeed) / 0.02; + lastSpeed = currentSpeed; + + Logger.recordOutput("Drive/Velocity", currentSpeed); + Logger.recordOutput("Drive/Acceleration", acceleration); + + Logger.recordOutput( + "Drive/NotZero", m_applyFieldSpeedsOrbit.getChassisSpeeds().vxMetersPerSecond != 0); + + Logger.recordOutput("Drive/ModuleTargets", getState().ModuleTargets); + Logger.recordOutput("Drive/ModulePositions", getState().ModulePositions); + Logger.recordOutput("Drive/ModuleStates", getState().ModuleStates); + + Logger.recordOutput("Drive/actualChassisSpeeds", getState().Speeds); } private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); m_simNotifier.startPeriodic(kSimLoopPeriod); } } diff --git a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java index ccd16dfa..df782bf8 100644 --- a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java +++ b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java @@ -9,17 +9,16 @@ import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; - public class FieldOrientedOrbitSwerveRequest implements SwerveRequest { - private final SwerveRequest.ApplyRobotSpeeds applyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDesaturateWheelSpeeds(false); + private final SwerveRequest.ApplyRobotSpeeds applyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDesaturateWheelSpeeds(false); private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); @@ -43,6 +42,10 @@ public ChassisSpeeds getChassisSpeeds() { return chassisSpeeds; } + public ChassisSpeeds getSlewedFieldChassisSpeeds() { + return slewedFieldChassisSpeeds; + } + public SwerveSetpoint getPreviousSetpoint() { return previousSetpoint; } @@ -52,12 +55,14 @@ public SwerveSetpoint getPreviousSetpoint() { * @param yTipLimiter * @param setpointGenerator * @param initialSetpoint - * - * This creates a FieldOrientedOrbitSwerveRequest. - * - * The anti-tipping logic currently does not actually work due to rotation and is taken out right now. + *

This creates a FieldOrientedOrbitSwerveRequest. + *

The anti-tipping logic currently does not actually work due to rotation and is taken + * out right now. */ - public FieldOrientedOrbitSwerveRequest(SwerveSetpointGenerator setpointGenerator, SwerveSetpoint initialSetpoint, Rotation2d robotOrientation) { + public FieldOrientedOrbitSwerveRequest( + SwerveSetpointGenerator setpointGenerator, + SwerveSetpoint initialSetpoint, + Rotation2d robotOrientation) { this.setpointGenerator = setpointGenerator; @@ -78,48 +83,78 @@ public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modu double xAcceleration = toApplyX - slewedFieldChassisSpeeds.vxMetersPerSecond; double yAcceleration = toApplyY - slewedFieldChassisSpeeds.vyMetersPerSecond; - + ChassisSpeeds accelerations = new ChassisSpeeds(xAcceleration, yAcceleration, 0); - accelerations = ChassisSpeeds.fromFieldRelativeSpeeds(accelerations,parameters.currentPose.getRotation()); - - if (accelerations.vxMetersPerSecond > forwardXRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vyMetersPerSecond = accelerations.vyMetersPerSecond * forwardXRateLimit*timestep / accelerations.vxMetersPerSecond; - accelerations.vxMetersPerSecond = forwardXRateLimit*timestep; - } else if (accelerations.vxMetersPerSecond < -backwardXRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vyMetersPerSecond = accelerations.vyMetersPerSecond * -backwardXRateLimit*timestep / accelerations.vxMetersPerSecond; - accelerations.vxMetersPerSecond = -backwardXRateLimit*timestep; + accelerations = + ChassisSpeeds.fromFieldRelativeSpeeds( + accelerations, parameters.currentPose.getRotation()); + + if (accelerations.vxMetersPerSecond > forwardXRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vyMetersPerSecond + * forwardXRateLimit + * timestep + / accelerations.vxMetersPerSecond; + accelerations.vxMetersPerSecond = forwardXRateLimit * timestep; + } else if (accelerations.vxMetersPerSecond < -backwardXRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vyMetersPerSecond + * -backwardXRateLimit + * timestep + / accelerations.vxMetersPerSecond; + accelerations.vxMetersPerSecond = -backwardXRateLimit * timestep; } - if (accelerations.vyMetersPerSecond > forwardYRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vyMetersPerSecond = accelerations.vxMetersPerSecond * forwardYRateLimit*timestep / accelerations.vyMetersPerSecond; - accelerations.vyMetersPerSecond = forwardYRateLimit*timestep; - } else if (accelerations.vyMetersPerSecond < -backwardYRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vxMetersPerSecond = accelerations.vxMetersPerSecond * -backwardYRateLimit*timestep / accelerations.vyMetersPerSecond; - accelerations.vyMetersPerSecond = -backwardYRateLimit*timestep; + if (accelerations.vyMetersPerSecond > forwardYRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vxMetersPerSecond + * forwardYRateLimit + * timestep + / accelerations.vyMetersPerSecond; + accelerations.vyMetersPerSecond = forwardYRateLimit * timestep; + } else if (accelerations.vyMetersPerSecond < -backwardYRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vxMetersPerSecond = + accelerations.vxMetersPerSecond + * -backwardYRateLimit + * timestep + / accelerations.vyMetersPerSecond; + accelerations.vyMetersPerSecond = -backwardYRateLimit * timestep; } - accelerations = ChassisSpeeds.fromFieldRelativeSpeeds(accelerations,parameters.currentPose.getRotation()); + accelerations = + ChassisSpeeds.fromRobotRelativeSpeeds( + accelerations, parameters.currentPose.getRotation()); slewedFieldChassisSpeeds = slewedFieldChassisSpeeds.plus(accelerations); - ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds(slewedFieldChassisSpeeds.vxMetersPerSecond, slewedFieldChassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); - - robotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(robotRelativeSpeeds,parameters.currentPose.getRotation()); + ChassisSpeeds robotRelativeSpeeds = + new ChassisSpeeds( + slewedFieldChassisSpeeds.vxMetersPerSecond, + slewedFieldChassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); - // Keep the robot from tipping over - // robotRelativeSpeeds.vxMetersPerSecond = xLimiter.calculate(robotRelativeSpeeds.vxMetersPerSecond); - // robotRelativeSpeeds.vyMetersPerSecond = yLimiter.calculate(robotRelativeSpeeds.vyMetersPerSecond); + robotRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + robotRelativeSpeeds, parameters.currentPose.getRotation()); // Apply all other limits - previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, robotRelativeSpeeds, timestep); + previousSetpoint = + setpointGenerator.generateSetpoint( + previousSetpoint, robotRelativeSpeeds, timestep, 12.0); DriveFeedforwards feedforwards = previousSetpoint.feedforwards(); - return applyRobotSpeeds - .withSpeeds(previousSetpoint.robotRelativeSpeeds()) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - .apply(parameters, modulesToApply); + ChassisSpeeds driveSpeeds = previousSetpoint.robotRelativeSpeeds(); + + return applyRobotSpeeds + .withSpeeds(driveSpeeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) + .apply(parameters, modulesToApply); } public FieldOrientedOrbitSwerveRequest withChassisSpeeds(ChassisSpeeds chassisSpeeds) { @@ -127,9 +162,11 @@ public FieldOrientedOrbitSwerveRequest withChassisSpeeds(ChassisSpeeds chassisSp return this; } - public FieldOrientedOrbitSwerveRequest withPreviousSetpoint(SwerveSetpoint previousSetpoint, Rotation2d robotOrientation) { + public FieldOrientedOrbitSwerveRequest withPreviousSetpoint( + SwerveSetpoint previousSetpoint, Rotation2d robotOrientation) { this.previousSetpoint = previousSetpoint; - this.slewedFieldChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, robotOrientation); + this.slewedFieldChassisSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, robotOrientation); return this; } @@ -138,13 +175,15 @@ public FieldOrientedOrbitSwerveRequest withDriverOrientation(boolean useDriverOr return this; } - public FieldOrientedOrbitSwerveRequest withXRateLimits(double forwardXRateLimit, double backwardXRateLimit) { + public FieldOrientedOrbitSwerveRequest withXRateLimits( + double forwardXRateLimit, double backwardXRateLimit) { this.forwardXRateLimit = forwardXRateLimit; this.backwardXRateLimit = backwardXRateLimit; return this; } - public FieldOrientedOrbitSwerveRequest withYRateLimits(double forwardYRateLimit, double backwardYRateLimit) { + public FieldOrientedOrbitSwerveRequest withYRateLimits( + double forwardYRateLimit, double backwardYRateLimit) { this.forwardYRateLimit = forwardYRateLimit; this.backwardYRateLimit = backwardYRateLimit; return this; @@ -155,8 +194,9 @@ public FieldOrientedOrbitSwerveRequest withTimestep(double timestep) { return this; } - public FieldOrientedOrbitSwerveRequest withMaintainStraightStopping(boolean maintainStraightStopping) { + public FieldOrientedOrbitSwerveRequest withMaintainStraightStopping( + boolean maintainStraightStopping) { this.maintainStraightStopping = maintainStraightStopping; return this; } -} \ No newline at end of file +}