Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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