mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
8f1f64ffb6
Also update copyright to include "and other WPILib contributors" and clarify license referral language to not be restricted to FIRST teams.
40 lines
1.4 KiB
C++
40 lines
1.4 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this 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 {};
|
|
}
|