diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp index 5a3d389e..df7c2dc7 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp @@ -155,6 +155,30 @@ frc::ChassisSpeeds RobotConfig::toChassisSpeeds( } } +frc::ChassisSpeeds RobotConfig::toChassisSpeeds( + std::vector states) const { + if (isHolonomic) { + wpi::array < frc::SwerveModuleState, 4 > wpiStates { states.at(0), + states.at(1), states.at(2), states.at(3) }; + return swerveKinematics.ToChassisSpeeds(wpiStates); + } else { + frc::DifferentialDriveWheelSpeeds wheelSpeeds { states.at(0).speed, + states.at(1).speed }; + return diffKinematics.ToChassisSpeeds(wheelSpeeds); + } +} + +std::vector RobotConfig::desaturateWheelSpeeds( + std::vector moduleStates, + units::meters_per_second_t maxSpeed) const { + wpi::array < frc::SwerveModuleState, 4 > wpiStates { moduleStates.at(0), + moduleStates.at(1), moduleStates.at(2), moduleStates.at(3) }; + swerveKinematics.DesaturateWheelSpeeds(&wpiStates, maxSpeed); + + return std::vector < frc::SwerveModuleState + > (wpiStates.begin(), wpiStates.end()); +} + std::vector RobotConfig::chassisForcesToWheelForceVectors( frc::ChassisSpeeds chassisForces) const { Eigen::Vector3d chassisForceVector { chassisForces.vx.value(), diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp new file mode 100644 index 00000000..19677adc --- /dev/null +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp @@ -0,0 +1,396 @@ +#include "pathplanner/lib/util/swerve/SwerveSetpointGenerator.h" + +SwerveSetpointGenerator::SwerveSetpointGenerator(RobotConfig *config, + units::radians_per_second_t maxSteerVelocity) { + this->config = config; + this->maxSteerVelocity = maxSteerVelocity; +} + +SwerveSetpoint SwerveSetpointGenerator::generateSetpoint( + SwerveSetpoint prevSetpoint, + frc::ChassisSpeeds desiredStateRobotRelative, units::second_t dt) { + std::vector < frc::SwerveModuleState > desiredModuleStates = + config->toSwerveModuleStates(desiredStateRobotRelative); + // Make sure desiredState respects velocity limits. + desiredModuleStates = config->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. + bool need_to_steer = true; + if (epsilonEquals(desiredStateRobotRelative, frc::ChassisSpeeds())) { + need_to_steer = false; + for (size_t m = 0; m < config->numModules; m++) { + desiredModuleStates[m].angle = prevSetpoint.moduleStates[m].angle; + desiredModuleStates[m].speed = 0_mps; + } + } + + // For each module, compute local Vx and Vy vectors. + units::meters_per_second_t prev_vx[4]; + units::meters_per_second_t prev_vy[4]; + frc::Rotation2d prev_heading[4]; + units::meters_per_second_t desired_vx[4]; + units::meters_per_second_t desired_vy[4]; + frc::Rotation2d desired_heading[4]; + bool all_modules_should_flip = true; + for (size_t m = 0; m < config->numModules; m++) { + prev_vx[m] = prevSetpoint.moduleStates[m].angle.Cos() + * prevSetpoint.moduleStates[m].speed; + prev_vy[m] = prevSetpoint.moduleStates[m].angle.Sin() + * prevSetpoint.moduleStates[m].speed; + prev_heading[m] = prevSetpoint.moduleStates[m].angle; + if (prevSetpoint.moduleStates[m].speed < 0.0_mps) { + prev_heading[m] = prev_heading[m].RotateBy( + frc::Rotation2d(180_deg)); + } + desired_vx[m] = desiredModuleStates[m].angle.Cos() + * desiredModuleStates[m].speed; + desired_vy[m] = desiredModuleStates[m].angle.Sin() + * desiredModuleStates[m].speed; + desired_heading[m] = desiredModuleStates[m].angle; + if (desiredModuleStates[m].speed < 0.0_mps) { + desired_heading[m] = desired_heading[m].RotateBy( + frc::Rotation2d(180_deg)); + } + if (all_modules_should_flip) { + units::radian_t required_rotation_rad = units::math::abs( + (-prev_heading[m].RotateBy(desired_heading[m])).Radians()); + if (required_rotation_rad < 90_deg) { + all_modules_should_flip = false; + } + } + } + + if (all_modules_should_flip + && !epsilonEquals(prevSetpoint.robotRelativeSpeeds, + frc::ChassisSpeeds()) + && !epsilonEquals(desiredStateRobotRelative, + frc::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, frc::ChassisSpeeds(), dt); + } + + // 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. + units::meters_per_second_t dx = desiredStateRobotRelative.vx + - prevSetpoint.robotRelativeSpeeds.vx; + units::meters_per_second_t dy = desiredStateRobotRelative.vy + - prevSetpoint.robotRelativeSpeeds.vy; + units::radians_per_second_t dtheta = desiredStateRobotRelative.omega + - prevSetpoint.robotRelativeSpeeds.omega; + + // '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). + std::vector < std::optional < frc::Rotation2d >> overrideSteering; + // 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 (size_t m = 0; m < config->numModules; m++) { + if (!need_to_steer) { + overrideSteering.push_back(prevSetpoint.moduleStates[m].angle); + continue; + } + overrideSteering.push_back(std::nullopt); + + units::radian_t max_theta_step = dt * maxSteerVelocity; + + if (epsilonEquals(prevSetpoint.moduleStates[m].speed.value(), 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].speed.value(), 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering[m] = prevSetpoint.moduleStates[m].angle; + continue; + } + + frc::Rotation2d neccesarryRotation = + (-prevSetpoint.moduleStates[m].angle).RotateBy( + desiredModuleStates[m].angle); + if (flipHeading(neccesarryRotation)) { + neccesarryRotation = neccesarryRotation.RotateBy( + frc::Rotation2d(180_deg)); + } + + // Radians() bounds to +/- Pi. + double numStepsNeeded = units::math::abs( + neccesarryRotation.Radians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + overrideSteering[m] = desiredModuleStates[m].angle; + } else { + overrideSteering[m] = + prevSetpoint.moduleStates[m].angle.RotateBy( + frc::Rotation2d( + max_theta_step + * (neccesarryRotation.Radians() + > 0_rad ? 1 : -1))); + 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. + units::radian_t maxHeadingChange { + (dt.value() * config->wheelFrictionForce.value()) + / ((config->mass.value() / config->numModules) + * units::math::abs( + prevSetpoint.moduleStates[m].speed).value()) }; + max_theta_step = units::math::min(max_theta_step, maxHeadingChange); + + double s = findSteeringMaxS(prev_vx[m], prev_vy[m], + prev_heading[m].Radians(), desired_vx[m], desired_vy[m], + desired_heading[m].Radians(), max_theta_step); + min_s = std::min(min_s, s); + } + + // Enforce drive wheel torque limits + frc::Translation2d chassisForceVec; + units::newton_meter_t chassisTorque { 0.0 }; + for (size_t m = 0; m < config->numModules; m++) { + units::radians_per_second_t lastVelRadPerSec { units::math::abs( + prevSetpoint.moduleStates[m].speed + / config->moduleConfig.wheelRadius).value() }; + units::volt_t inputVoltage { frc::RobotController::GetInputVoltage() }; + // 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 + units::ampere_t currentDraw = config->moduleConfig.driveMotor.Current( + lastVelRadPerSec, inputVoltage); + currentDraw = std::min(currentDraw, + config->moduleConfig.driveCurrentLimit); + units::newton_meter_t moduleTorque = + config->moduleConfig.driveMotor.Torque(currentDraw); + + units::meters_per_second_t prevSpeed = + prevSetpoint.moduleStates[m].speed; + desiredModuleStates[m].Optimize(prevSetpoint.moduleStates[m].angle); + units::meters_per_second_t desiredSpeed = desiredModuleStates[m].speed; + + int forceSign; + frc::Rotation2d forceAngle = prevSetpoint.moduleStates[m].angle; + if (epsilonEquals(prevSpeed.value(), 0) + || (prevSpeed > 0_mps && desiredSpeed >= prevSpeed) + || (prevSpeed < 0_mps && desiredSpeed <= prevSpeed)) { + // Torque loss will be fighting motor + moduleTorque -= config->moduleConfig.torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0_mps) { + forceAngle = forceAngle + frc::Rotation2d(180_deg); + } + } else { + // 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_mps) { + forceAngle = forceAngle + frc::Rotation2d(180_deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = std::min(moduleTorque, config->maxTorqueFriction); + + units::newton_t forceAtCarpet = moduleTorque + / config->moduleConfig.wheelRadius; + frc::Translation2d moduleForceVec = { ((forceAtCarpet * forceSign) + / 1_kg) * 1_s * 1_s, forceAngle }; + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec + moduleForceVec; + + // Calculate the torque this module will apply to the chassis + frc::Rotation2d angleToModule = config->moduleLocations[m].Angle(); + frc::Rotation2d theta = moduleForceVec.Angle() - angleToModule; + chassisTorque += forceAtCarpet * config->modulePivotDistance[m] + * theta.Sin(); + } + + frc::Translation2d chassisAccelVec = chassisForceVec / config->mass.value(); + units::radians_per_second_squared_t chassisAngularAccel { + chassisTorque.value() / config->MOI.value() }; + + // Use kinematics to convert chassis accelerations to module accelerations + frc::ChassisSpeeds chassisAccel { chassisAccelVec.X() / 1_s, + chassisAccelVec.Y() / 1_s, chassisAngularAccel * 1_s }; + auto accelStates = config->toSwerveModuleStates(chassisAccel); + + for (size_t m = 0; m < config->numModules; m++) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + + units::meters_per_second_t maxVelStep = units::math::abs( + accelStates[m].speed * dt.value()); + + units::meters_per_second_t vx_min_s = + min_s == 1.0 ? + desired_vx[m] : + (desired_vx[m] - prev_vx[m]) * min_s + prev_vx[m]; + units::meters_per_second_t 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], + units::math::hypot(prev_vx[m], prev_vy[m]), vx_min_s, vy_min_s, + units::math::hypot(vx_min_s, vy_min_s), maxVelStep); + min_s = std::min(min_s, s); + } + + frc::ChassisSpeeds retSpeeds = { prevSetpoint.robotRelativeSpeeds.vx + + min_s * dx, prevSetpoint.robotRelativeSpeeds.vy + min_s * dy, + prevSetpoint.robotRelativeSpeeds.omega + min_s * dtheta }; + retSpeeds = frc::ChassisSpeeds::Discretize(retSpeeds, dt); + + units::meters_per_second_t prevVelX = prevSetpoint.robotRelativeSpeeds.vx; + units::meters_per_second_t prevVelY = prevSetpoint.robotRelativeSpeeds.vy; + units::meters_per_second_squared_t chassisAccelX = (retSpeeds.vx - prevVelX) + / dt; + units::meters_per_second_squared_t chassisAccelY = (retSpeeds.vy - prevVelY) + / dt; + units::newton_t chassisForceX = chassisAccelX * config->mass; + units::newton_t chassisForceY = chassisAccelY * config->mass; + + units::radians_per_second_squared_t angularAccel = (retSpeeds.omega + - prevSetpoint.robotRelativeSpeeds.omega) / dt; + units::newton_meter_t angTorque { angularAccel.value() * config->MOI.value() }; + frc::ChassisSpeeds chassisForces { chassisForceX * 1_s / 1_kg, chassisForceY + * 1_s / 1_kg, units::radians_per_second_t { angTorque.value() } }; + + std::vector < frc::Translation2d > wheelForces = + config->chassisForcesToWheelForceVectors(chassisForces); + + std::vector < frc::SwerveModuleState > retStates = + config->toSwerveModuleStates(chassisForces); + std::vector < units::meters_per_second_squared_t + > accelFF(config->numModules); + std::vector < units::newton_t > linearForceFF(config->numModules); + std::vector < units::ampere_t > torqueCurrentFF(config->numModules); + std::vector < units::newton_t > forceXFF(config->numModules); + std::vector < units::newton_t > forceYFF(config->numModules); + for (size_t m = 0; m < config->numModules; m++) { + units::meter_t wheelForceDist = wheelForces[m].Norm(); + units::newton_t appliedForce = + wheelForceDist > 1e-6_m ? + units::newton_t { + wheelForceDist.value() + * (wheelForces[m].Angle() + - retStates[m].angle).Cos() } : + 0_N; + units::newton_meter_t wheelTorque = appliedForce + * config->moduleConfig.wheelRadius; + units::ampere_t torqueCurrent = config->moduleConfig.driveMotor.Current( + wheelTorque); + + std::optional < frc::Rotation2d > maybeOverride = overrideSteering[m]; + if (maybeOverride.has_value()) { + frc::Rotation2d override = maybeOverride.value(); + if (flipHeading((-retStates[m].angle).RotateBy(override))) { + retStates[m].speed = -retStates[m].speed; + appliedForce = -appliedForce; + torqueCurrent = -torqueCurrent; + } + retStates[m].angle = override; + } + frc::Rotation2d deltaRotation = + (-prevSetpoint.moduleStates[m].angle).RotateBy( + retStates[m].angle); + if (flipHeading(deltaRotation)) { + retStates[m].angle = retStates[m].angle.RotateBy( + frc::Rotation2d(180_deg)); + retStates[m].speed = -retStates[m].speed; + appliedForce = -appliedForce; + torqueCurrent = -torqueCurrent; + } + + accelFF[m] = (retStates[m].speed - prevSetpoint.moduleStates[m].speed) + / dt; + linearForceFF[m] = appliedForce; + torqueCurrentFF[m] = torqueCurrent; + forceXFF[m] = wheelForces[m].X().value() * 1_N; + forceYFF[m] = wheelForces[m].Y().value() * 1_N; + } + + return SwerveSetpoint { retSpeeds, retStates, DriveFeedforwards { accelFF, + linearForceFF, torqueCurrentFF, forceXFF, forceYFF } }; +} + +double SwerveSetpointGenerator::findRoot(Function2d func, double x_0, + double y_0, double f_0, double x_1, double y_1, double f_1, + int iterations_left) { + auto s_guess = std::max(0.0, std::min(1.0, -f_0 / (f_1 - f_0))); + + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return s_guess; + } + + double x_guess = (x_1 - x_0) * s_guess + x_0; + double y_guess = (y_1 - y_0) * s_guess + y_0; + double f_guess = func(x_guess, y_guess); + + if (std::signbit(f_0) == std::signbit(f_guess)) { + 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); + } +} + +double SwerveSetpointGenerator::findSteeringMaxS(units::meters_per_second_t x_0, + units::meters_per_second_t y_0, units::radian_t f_0, + units::meters_per_second_t x_1, units::meters_per_second_t y_1, + units::radian_t f_1, units::radian_t max_deviation) { + f_1 = unwrapAngle(f_0.value(), f_1.value()); + units::radian_t diff = f_1 - f_0; + if (units::math::abs(diff) < max_deviation) { + return 1.0; + } + units::radian_t offset = f_0 + std::signbit(diff.value()) * max_deviation; + Function2d func = [this, f_0, offset](double x, double y) -> double { + return (unwrapAngle(f_0.value(), std::atan2(y, x)) - offset).value(); + }; + return findRoot(func, x_0.value(), y_0.value(), + f_0.value() - offset.value(), x_1.value(), y_1.value(), + f_1.value() - offset.value(), MAX_STEER_ITERATIONS); +} + +double SwerveSetpointGenerator::findDriveMaxS(units::meters_per_second_t x_0, + units::meters_per_second_t y_0, units::meters_per_second_t f_0, + units::meters_per_second_t x_1, units::meters_per_second_t y_1, + units::meters_per_second_t f_1, + units::meters_per_second_t max_vel_step) { + units::meters_per_second_t diff = f_1 - f_0; + if (units::math::abs(diff) < max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + units::meters_per_second_t offset = f_0 + + std::signbit(diff.value()) * max_vel_step; + Function2d func = [this, offset](double x, double y) -> double { + return std::hypot(y, x) - offset.value(); + }; + return findRoot(func, x_0.value(), y_0.value(), + f_0.value() - offset.value(), x_1.value(), y_1.value(), + f_1.value() - offset.value(), MAX_DRIVE_ITERATIONS); +} diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/config/RobotConfig.h b/pathplannerlib/src/main/native/include/pathplanner/lib/config/RobotConfig.h index 888c680a..02209256 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/config/RobotConfig.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/config/RobotConfig.h @@ -57,6 +57,27 @@ class RobotConfig { frc::ChassisSpeeds toChassisSpeeds( std::vector states) const; + /** + * Convert a vector of swerve module states to robot-relative chassis speeds. This will use + * differential kinematics for diff drive robots. + * + * @param states Vector of swerve module states + * @return Robot-relative chassis speeds + */ + frc::ChassisSpeeds toChassisSpeeds( + std::vector states) const; + + /** + * Desaturate wheel speeds to respect velocity limits. + * + * @param moduleStates The module states to desaturate + * @param maxSpeed The maximum speed that the robot can reach while actually driving the robot at full output + * @return The desaturated module states + */ + std::vector desaturateWheelSpeeds( + std::vector moduleStates, + units::meters_per_second_t maxSpeed) const; + std::vector chassisForcesToWheelForceVectors( frc::ChassisSpeeds chassisForces) const; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpoint.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpoint.h new file mode 100644 index 00000000..487219ff --- /dev/null +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpoint.h @@ -0,0 +1,22 @@ +#pragma once + +#include "pathplanner/lib/util/DriveFeedforwards.h" +#include +#include + +/** + * A setpoint for a swerve drivetrain, containing robot-relative chassis speeds and individual + * module states + * + * @param robotRelativeSpeeds Robot-relative chassis speeds + * @param moduleStates Array of individual swerve module states. These will be in FL, FR, BL, BR + * order. + * @param feedforwards Feedforwards for each module's drive motor. The arrays in this record will be + * in FL, FR, BL, BR order. + */ +struct SwerveSetpoint { +public: + frc::ChassisSpeeds robotRelativeSpeeds; + std::vector moduleStates; + pathplanner::DriveFeedforwards feedforwards; +}; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpointGenerator.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpointGenerator.h new file mode 100644 index 00000000..f85e041a --- /dev/null +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpointGenerator.h @@ -0,0 +1,123 @@ +#include "pathplanner/lib/config/RobotConfig.h" +#include "pathplanner/lib/util/DriveFeedforwards.h" +#include "pathplanner/lib/util/swerve/SwerveSetpoint.h" + +#include +#include +#include + +using namespace pathplanner; + +/** + * 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. + */ +class SwerveSetpointGenerator { +public: + + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocity The maximum rotation velocity of a swerve module, in radians + * per second + */ + SwerveSetpointGenerator(RobotConfig *config, + units::radians_per_second_t maxSteerVelocity); + + /** + * Generate a new setpoint. 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. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + SwerveSetpoint generateSetpoint(SwerveSetpoint prevSetpoint, + frc::ChassisSpeeds desiredStateRobotRelative, units::second_t dt); + + /** + * 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. + */ + inline static bool flipHeading(frc::Rotation2d prevToGoal) { + return units::math::abs(prevToGoal.Radians()).value() > PI / 2.0; + } + + inline static units::radian_t unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > PI) { + return units::radian_t(angle - 2.0 * PI); + } else if (diff < -PI) { + return units::radian_t(angle + 2.0 * PI); + } else { + return units::radian_t(angle); + } + } + +private: + double kEpsilon = 1e-8; + int MAX_STEER_ITERATIONS = 8; + int MAX_DRIVE_ITERATIONS = 10; + + RobotConfig *config = nullptr; + units::radians_per_second_t maxSteerVelocity; + using Function2d = std::function; + + /** + * 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. + */ + 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); + double findSteeringMaxS(units::meters_per_second_t x_0, + units::meters_per_second_t y_0, units::radian_t f_0, + units::meters_per_second_t x_1, units::meters_per_second_t y_1, + units::radian_t f_1, units::radian_t max_deviation); + double findDriveMaxS(units::meters_per_second_t x_0, + units::meters_per_second_t y_0, units::meters_per_second_t f_0, + units::meters_per_second_t x_1, units::meters_per_second_t y_1, + units::meters_per_second_t f_1, + units::meters_per_second_t max_vel_step); + + inline bool epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + inline bool epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + inline bool epsilonEquals(frc::ChassisSpeeds s1, frc::ChassisSpeeds s2) { + return epsilonEquals(s1.vx.to(), s2.vx.to()) + && epsilonEquals(s1.vy.to(), s2.vy.to()) + && epsilonEquals(s1.omega.to(), s2.omega.to()); + } + +};