Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilibc] Fix const-qualification in kinematics and constraints (#2478)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-04-14 01:32:25 -04:00
committed by GitHub
parent a3a8472b82
commit e5935a4737
21 changed files with 71 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
View File

@@ -15,7 +15,7 @@ CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
// ac = v^2 / r
// k (curvature) = 1 / r
@@ -33,7 +33,7 @@ units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
TrajectoryConstraint::MinMax
CentripetalAccelerationConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return {};

4
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
View File

@@ -15,7 +15,7 @@ DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Normalize(m_maxSpeed);
@@ -26,6 +26,6 @@ units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
TrajectoryConstraint::MinMax
DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
return {};
}

4
wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
View File

@@ -23,14 +23,14 @@ DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
return units::meters_per_second_t(std::numeric_limits<double>::max());
}
TrajectoryConstraint::MinMax
DifferentialDriveVoltageConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});

8
wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
View File

@@ -13,7 +13,7 @@ using namespace frc;
EllipticalRegionConstraint::EllipticalRegionConstraint(
const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
const Rotation2d& rotation, TrajectoryConstraint& constraint)
const Rotation2d& rotation, const TrajectoryConstraint& constraint)
: m_center(center),
m_radii(xWidth / 2.0, yWidth / 2.0),
m_constraint(constraint) {
@@ -22,7 +22,7 @@ EllipticalRegionConstraint::EllipticalRegionConstraint(
units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
@@ -32,7 +32,7 @@ units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
@@ -40,7 +40,7 @@ TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
}
}
bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
// The region (disk) bounded by the ellipse is given by the equation:
// ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
// If the inequality is satisfied, then it is inside the ellipse; otherwise

4
wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
View File

@@ -15,7 +15,7 @@ MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds =
@@ -30,6 +30,6 @@ units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
TrajectoryConstraint::MinMax
MecanumDriveKinematicsConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
return {};
}

8
wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
View File

@@ -13,14 +13,14 @@ using namespace frc;
RectangularRegionConstraint::RectangularRegionConstraint(
const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
TrajectoryConstraint& constraint)
const TrajectoryConstraint& constraint)
: m_bottomLeftPoint(bottomLeftPoint),
m_topRightPoint(topRightPoint),
m_constraint(constraint) {}
units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) {
units::meters_per_second_t velocity) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
@@ -30,7 +30,7 @@ units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) {
units::meters_per_second_t speed) const {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
@@ -38,7 +38,7 @@ TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
}
}
bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
return pose.Translation().X() >= m_bottomLeftPoint.X() &&
pose.Translation().X() <= m_topRightPoint.X() &&
pose.Translation().Y() >= m_bottomLeftPoint.Y() &&