mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
42993b15c6
The wpimath library is a new library designed to separate the reusable math functionality from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j). Package names / include file names were NOT changed to minimize breakage. In a future year it would be good to revamp these for a more uniform user experience and to reduce the risk of accidental naming conflicts. While theoretically all of this functionality could be placed into wpiutil, several pieces of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this expense for users who only want cscore or ntcore. It also allows for easy future separation of build tasks vs number of workers on memory-constrained machines. This moves the following functionality from wpiutil into wpimath: - Eigen - ejml - Drake - DARE - wpiutil.math package (Matrix etc) - units And the following functionality from wpilibc/j into wpimath: - Geometry - Kinematics - Spline - Trajectory - LinearFilter - MedianFilter - Feed-forward controllers
43 lines
1.7 KiB
C++
43 lines
1.7 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
|
|
|
|
#include "units/math.h"
|
|
|
|
using namespace frc;
|
|
|
|
CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
|
|
units::meters_per_second_squared_t maxCentripetalAcceleration)
|
|
: m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
|
|
|
|
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
|
|
const Pose2d& pose, units::curvature_t curvature,
|
|
units::meters_per_second_t velocity) const {
|
|
// ac = v^2 / r
|
|
// k (curvature) = 1 / r
|
|
|
|
// therefore, ac = v^2 * k
|
|
// ac / k = v^2
|
|
// v = std::sqrt(ac / k)
|
|
|
|
// We have to multiply by 1_rad here to get the units to cancel out nicely.
|
|
// The units library defines a unit for radians although it is technically
|
|
// unitless.
|
|
return units::math::sqrt(m_maxCentripetalAcceleration /
|
|
units::math::abs(curvature) * 1_rad);
|
|
}
|
|
|
|
TrajectoryConstraint::MinMax
|
|
CentripetalAccelerationConstraint::MinMaxAcceleration(
|
|
const Pose2d& pose, units::curvature_t curvature,
|
|
units::meters_per_second_t speed) const {
|
|
// The acceleration of the robot has no impact on the centripetal acceleration
|
|
// of the robot.
|
|
return {};
|
|
}
|