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

Add DifferentialDrive voltage constraint (#2075)

Browse Source
This commit is contained in:
Oblarg
2019-11-22 00:43:02 -05:00
committed by Peter Johnson
parent e0bc97f66b
commit 21e957ee42
17 changed files with 868 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -40,7 +40,7 @@ class ArmFeedforward {
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: m_kS(kS), m_kCos(kCos), m_kV(kV), m_kA(kA) {}
: kS(kS), kCos(kCos), kV(kV), kA(kA) {}
/**
* Calculates the feedforward from the gains and setpoints.
@@ -54,14 +54,94 @@ class ArmFeedforward {
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
return m_kS * wpi::sgn(velocity) + m_kCos * units::math::cos(angle) +
m_kV * velocity + m_kA * acceleration;
return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
kV * velocity + kA * acceleration;
}
private:
units::volt_t m_kS{0};
units::volt_t m_kCos{0};
units::unit_t<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
/**
* Calculates the maximum achievable velocity given a maximum voltage supply,
* a position, and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param acceleration The acceleration of the arm.
* @return The maximum possible velocity at the given acceleration and angle.
*/
units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, Angle angle,
units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kCos * units::math::cos(angle) -
kA * acceleration) /
kV;
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply,
* a position, and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param acceleration The acceleration of the arm.
* @return The minimum possible velocity at the given acceleration and angle.
*/
units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, Angle angle,
units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kCos * units::math::cos(angle) -
kA * acceleration) /
kV;
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply, a position, and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param velocity The velocity of the arm.
* @return The maximum possible acceleration at the given velocity and angle.
*/
units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, Angle angle, units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) -
kCos * units::math::cos(angle) - kV * velocity) /
kA;
}
/**
* Calculates the minimum achievable acceleration given a maximum voltage
* supply, a position, and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param velocity The velocity of the arm.
* @return The minimum possible acceleration at the given velocity and angle.
*/
units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, Angle angle, units::unit_t<Velocity> velocity) {
return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
}
units::volt_t kS{0};
units::volt_t kCos{0};
units::unit_t<kv_unit> kV{0};
units::unit_t<ka_unit> kA{0};
};
} // namespace frc

87
wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -26,7 +26,7 @@ class ElevatorFeedforward {
units::compound_unit<units::volts, units::inverse<Acceleration>>;
public:
constexpr ElevatorFeedforward() = default;
ElevatorFeedforward() = default;
/**
* Creates a new ElevatorFeedforward with the specified gains.
@@ -39,7 +39,7 @@ class ElevatorFeedforward {
constexpr ElevatorFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: m_kS(kS), m_kG(kG), m_kV(kV), m_kA(kA) {}
: kS(kS), kG(kG), kV(kV), kA(kA) {}
/**
* Calculates the feedforward from the gains and setpoints.
@@ -50,15 +50,82 @@ class ElevatorFeedforward {
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
return m_kS * wpi::sgn(velocity) + m_kG + m_kV * velocity +
m_kA * acceleration;
units::unit_t<Acceleration>(0)) {
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
}
private:
units::volt_t m_kS{0};
units::volt_t m_kG{0};
units::unit_t<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
/**
* Calculates the maximum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param acceleration The acceleration of the elevator.
* @return The maximum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kG - kA * acceleration) / kV;
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param acceleration The acceleration of the elevator.
* @return The minimum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kG - kA * acceleration) / kV;
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param velocity The velocity of the elevator.
* @return The maximum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
}
/**
* Calculates the minimum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param velocity The velocity of the elevator.
* @return The minimum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}
units::volt_t kS{0};
units::volt_t kG{0};
units::unit_t<kv_unit> kV{0};
units::unit_t<ka_unit> kA{0};
};
} // namespace frc

80
wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -38,7 +38,7 @@ class SimpleMotorFeedforward {
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: m_kS(kS), m_kV(kV), m_kA(kA) {}
: kS(kS), kV(kV), kA(kA) {}
/**
* Calculates the feedforward from the gains and setpoints.
@@ -50,12 +50,80 @@ class SimpleMotorFeedforward {
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
return m_kS * wpi::sgn(velocity) + m_kV * velocity + m_kA * acceleration;
return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
}
private:
units::volt_t m_kS{0};
units::unit_t<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
/**
* Calculates the maximum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param acceleration The acceleration of the motor.
* @return The maximum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kA * acceleration) / kV;
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param acceleration The acceleration of the motor.
* @return The minimum possible velocity at the given acceleration.
*/
constexpr units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
// Assume min velocity is positive, ks flips sign
return (-maxVoltage + kS - kA * acceleration) / kV;
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param velocity The velocity of the motor.
* @return The maximum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
}
/**
* Calculates the minimum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param velocity The velocity of the motor.
* @return The minimum possible acceleration at the given velocity.
*/
constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}
units::volt_t kS{0};
units::unit_t<kv_unit> kV{0};
units::unit_t<ka_unit> kA{0};
};
} // namespace frc

11
wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
View File

@@ -32,7 +32,7 @@ class DifferentialDriveKinematics {
* scrubbing effects.
*/
constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
: m_trackWidth(trackWidth) {}
: trackWidth(trackWidth) {}
/**
* Returns a chassis speed from left and right component velocities using
@@ -44,7 +44,7 @@ class DifferentialDriveKinematics {
constexpr ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
(wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
}
/**
@@ -57,11 +57,10 @@ class DifferentialDriveKinematics {
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const {
return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
}
private:
units::meter_t m_trackWidth;
units::meter_t trackWidth;
};
} // namespace frc

51
wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h Normal file
View File

@@ -0,0 +1,51 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
#include "frc/controller/SimpleMotorFeedforward.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* A class that enforces constraints on differential drive voltage expenditure
* based on the motor dynamics and the drive kinematics. Ensures that the
* acceleration of any wheel of the robot while following the trajectory is
* never higher than what can be achieved with the given maximum voltage.
*/
class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
public:
/**
* Creates a new DifferentialDriveVoltageConstraint.
*
* @param feedforward A feedforward component describing the behavior of the
* drive.
* @param kinematics A kinematics component describing the drive geometry.
* @param maxVoltage The maximum voltage available to the motors while
* following the path. Should be somewhat less than the nominal battery
* voltage (12V) to account for "voltage sag" due to current draw.
*/
DifferentialDriveVoltageConstraint(
SimpleMotorFeedforward<units::meter> feedforward,
DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, curvature_t curvature,
units::meters_per_second_t velocity) override;
MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
units::meters_per_second_t speed) override;
private:
SimpleMotorFeedforward<units::meter> m_feedforward;
DifferentialDriveKinematics m_kinematics;
units::volt_t m_maxVoltage;
};
} // namespace frc