mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
41 lines
1.6 KiB
C++
41 lines
1.6 KiB
C++
|
/*----------------------------------------------------------------------------*/
|
||
|
|
/* Copyright (c) 2019 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"
|
||
|
|
|
||
|
|
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, curvature_t curvature,
|
||
|
|
units::meters_per_second_t velocity) {
|
||
|
|
// 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, curvature_t curvature,
|
||
|
|
units::meters_per_second_t speed) {
|
||
|
|
// The acceleration of the robot has no impact on the centripetal acceleration
|
||
|
|
// of the robot.
|
||
|
|
return {};
|
||
|
|
}
|