diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp new file mode 100644 index 0000000000..3d6b61c50b --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* 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/DifferentialDriveVoltageConstraint.h" + +#include +#include + +#include + +using namespace frc; + +DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint( + SimpleMotorFeedforward feedforward, + DifferentialDriveKinematics kinematics, units::volt_t maxVoltage) + : m_feedforward(feedforward), + m_kinematics(kinematics), + m_maxVoltage(maxVoltage) {} + +units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t velocity) { + return units::meters_per_second_t(std::numeric_limits::max()); +} + +TrajectoryConstraint::MinMax +DifferentialDriveVoltageConstraint::MinMaxAcceleration( + const Pose2d& pose, curvature_t curvature, + units::meters_per_second_t speed) { + auto wheelSpeeds = + m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature}); + + auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right); + auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right); + + // Calculate maximum/minimum possible accelerations from motor dynamics + // and max/min wheel speeds + auto maxWheelAcceleration = + m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed); + auto minWheelAcceleration = + m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed); + + // Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius + // increased by half of the trackwidth T. Inner wheel has radius decreased + // by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so + // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2) + // Inner wheel is similar. + + // sgn(speed) term added to correctly account for which wheel is on + // outside of turn: + // If moving forward, max acceleration constraint corresponds to wheel on + // outside of turn + // If moving backward, max acceleration constraint corresponds to wheel on + // inside of turn + auto maxChassisAcceleration = + maxWheelAcceleration / + (1 + m_kinematics.trackWidth * units::math::abs(curvature) * + wpi::sgn(speed) / (2_rad)); + auto minChassisAcceleration = + minWheelAcceleration / + (1 - m_kinematics.trackWidth * units::math::abs(curvature) * + wpi::sgn(speed) / (2_rad)); + + // Negate acceleration corresponding to wheel on inside of turn + // if center of turn is inside of wheelbase + if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) { + if (speed > 0_mps) { + minChassisAcceleration = -minChassisAcceleration; + } else { + maxChassisAcceleration = -maxChassisAcceleration; + } + } + + return {minChassisAcceleration, maxChassisAcceleration}; +} diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h index d1a8ceac9b..3dc962b790 100644 --- a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h @@ -40,7 +40,7 @@ class ArmFeedforward { constexpr ArmFeedforward( units::volt_t kS, units::volt_t kCos, units::unit_t kV, units::unit_t kA = units::unit_t(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, units::unit_t acceleration = units::unit_t(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 m_kV{0}; - units::unit_t 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 MaxAchievableVelocity( + units::volt_t maxVoltage, Angle angle, + units::unit_t 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 MinAchievableVelocity( + units::volt_t maxVoltage, Angle angle, + units::unit_t 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 MaxAchievableAcceleration( + units::volt_t maxVoltage, Angle angle, units::unit_t 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 MinAchievableAcceleration( + units::volt_t maxVoltage, Angle angle, units::unit_t velocity) { + return MaxAchievableAcceleration(-maxVoltage, angle, velocity); + } + + units::volt_t kS{0}; + units::volt_t kCos{0}; + units::unit_t kV{0}; + units::unit_t kA{0}; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h index 9938b7b46c..c664fc40b6 100644 --- a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -26,7 +26,7 @@ class ElevatorFeedforward { units::compound_unit>; 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, units::unit_t kA = units::unit_t(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, units::unit_t acceleration = - units::unit_t(0)) const { - return m_kS * wpi::sgn(velocity) + m_kG + m_kV * velocity + - m_kA * acceleration; + units::unit_t(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 m_kV{0}; - units::unit_t 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 MaxAchievableVelocity( + units::volt_t maxVoltage, units::unit_t 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 MinAchievableVelocity( + units::volt_t maxVoltage, units::unit_t 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 MaxAchievableAcceleration( + units::volt_t maxVoltage, units::unit_t 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 MinAchievableAcceleration( + units::volt_t maxVoltage, units::unit_t velocity) { + return MaxAchievableAcceleration(-maxVoltage, velocity); + } + + units::volt_t kS{0}; + units::volt_t kG{0}; + units::unit_t kV{0}; + units::unit_t kA{0}; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 03aebf2c07..8f82cb9ba6 100644 --- a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -38,7 +38,7 @@ class SimpleMotorFeedforward { constexpr SimpleMotorFeedforward( units::volt_t kS, units::unit_t kV, units::unit_t kA = units::unit_t(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, units::unit_t acceleration = units::unit_t(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 m_kV{0}; - units::unit_t 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 MaxAchievableVelocity( + units::volt_t maxVoltage, units::unit_t 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 MinAchievableVelocity( + units::volt_t maxVoltage, units::unit_t 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 MaxAchievableAcceleration( + units::volt_t maxVoltage, units::unit_t 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 MinAchievableAcceleration( + units::volt_t maxVoltage, units::unit_t velocity) { + return MaxAchievableAcceleration(-maxVoltage, velocity); + } + + units::volt_t kS{0}; + units::unit_t kV{0}; + units::unit_t kA{0}; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 7db0485328..cb2121f8de 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -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 diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h new file mode 100644 index 0000000000..50ace85b85 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h @@ -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 + +#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 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 m_feedforward; + DifferentialDriveKinematics m_kinematics; + units::volt_t m_maxVoltage; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp new file mode 100644 index 0000000000..eb230dac39 --- /dev/null +++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h" +#include "gtest/gtest.h" +#include "trajectory/TestTrajectory.h" + +using namespace frc; + +TEST(DifferentialDriveVoltageConstraintTest, Constraint) { + // Pick an unreasonably large kA to ensure the constraint has to do some work + SimpleMotorFeedforward feedforward{1_V, 1_V / 1_mps, + 3_V / 1_mps_sq}; + const DifferentialDriveKinematics kinematics{0.5_m}; + const auto maxVoltage = 10_V; + + auto config = TrajectoryConfig(12_fps, 12_fps_sq); + config.AddConstraint( + DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage)); + + auto trajectory = TestTrajectory::GetTrajectory(config); + + units::second_t time = 0_s; + units::second_t dt = 20_ms; + units::second_t duration = trajectory.TotalTime(); + + while (time < duration) { + const Trajectory::State point = trajectory.Sample(time); + time += dt; + + const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps, + point.velocity * point.curvature}; + + auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds); + + // Not really a strictly-correct test as we're using the chassis accel + // instead of the wheel accel, but much easier than doing it "properly" and + // a reasonable check anyway + EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) < + maxVoltage + 0.05_V); + EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) > + -maxVoltage - 0.05_V); + EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) < + maxVoltage + 0.05_V); + EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) > + -maxVoltage - 0.05_V); + } +} diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index bd03d5b04b..71da41081a 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,11 +44,19 @@ void RobotContainer::ConfigureButtonBindings() { } frc2::Command* RobotContainer::GetAutonomousCommand() { + // Create a voltage constraint to ensure we don't accelerate too fast + frc::DifferentialDriveVoltageConstraint autoVoltageConstraint( + frc::SimpleMotorFeedforward( + DriveConstants::ks, DriveConstants::kv, DriveConstants::ka), + DriveConstants::kDriveKinematics, 10_V); + // Set up config for trajectory frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, AutoConstants::kMaxAcceleration); // Add kinematics to ensure max speed is actually obeyed config.SetKinematics(DriveConstants::kDriveKinematics); + // Apply the voltage constraint + config.AddConstraint(autoVoltageConstraint); // An example trajectory to follow. All units in meters. auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory( diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java index 786e7e2f83..355b182885 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java @@ -11,11 +11,12 @@ package edu.wpi.first.wpilibj.controller; * A helper class that computes feedforward outputs for a simple arm (modeled as a motor * acting against the force of gravity on a beam suspended at an angle). */ +@SuppressWarnings("MemberName") public class ArmFeedforward { - private final double m_ks; - private final double m_kcos; - private final double m_kv; - private final double m_ka; + public final double ks; + public final double kcos; + public final double kv; + public final double ka; /** * Creates a new ArmFeedforward with the specified gains. Units of the gain values @@ -27,10 +28,10 @@ public class ArmFeedforward { * @param ka The acceleration gain. */ public ArmFeedforward(double ks, double kcos, double kv, double ka) { - m_ks = ks; - m_kcos = kcos; - m_kv = kv; - m_ka = ka; + this.ks = ks; + this.kcos = kcos; + this.kv = kv; + this.ka = ka; } /** @@ -54,9 +55,9 @@ public class ArmFeedforward { */ public double calculate(double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) { - return m_ks * Math.signum(velocityRadPerSec) + m_kcos * Math.cos(positionRadians) - + m_kv * velocityRadPerSec - + m_ka * accelRadPerSecSquared; + return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians) + + kv * velocityRadPerSec + + ka * accelRadPerSecSquared; } /** @@ -69,4 +70,73 @@ public class ArmFeedforward { public double calculate(double positionRadians, double velocity) { return calculate(positionRadians, velocity, 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. + */ + public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) { + // Assume max velocity is positive + return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / 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. + */ + public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) { + // Assume min velocity is negative, ks flips sign + return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / 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. + */ + public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { + return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / 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. + */ + public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { + return maxAchievableAcceleration(-maxVoltage, angle, velocity); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java index 86fa801085..86a4f7e481 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java @@ -11,11 +11,12 @@ package edu.wpi.first.wpilibj.controller; * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor * acting against the force of gravity). */ +@SuppressWarnings("MemberName") public class ElevatorFeedforward { - private final double m_ks; - private final double m_kg; - private final double m_kv; - private final double m_ka; + public final double ks; + public final double kg; + public final double kv; + public final double ka; /** * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values @@ -27,10 +28,10 @@ public class ElevatorFeedforward { * @param ka The acceleration gain. */ public ElevatorFeedforward(double ks, double kg, double kv, double ka) { - m_ks = ks; - m_kg = kg; - m_kv = kv; - m_ka = ka; + this.ks = ks; + this.kg = kg; + this.kv = kv; + this.ka = ka; } /** @@ -53,7 +54,7 @@ public class ElevatorFeedforward { * @return The computed feedforward. */ public double calculate(double velocity, double acceleration) { - return m_ks * Math.signum(velocity) + m_kg + m_kv * velocity + m_ka * acceleration; + return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; } /** @@ -66,4 +67,69 @@ public class ElevatorFeedforward { public double calculate(double velocity) { return calculate(velocity, 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. + */ + public double maxAchievableVelocity(double maxVoltage, double acceleration) { + // Assume max velocity is positive + return (maxVoltage - ks - kg - acceleration * ka) / 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. + */ + public double minAchievableVelocity(double maxVoltage, double acceleration) { + // Assume min velocity is negative, ks flips sign + return (-maxVoltage + ks - kg - acceleration * ka) / 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. + */ + public double maxAchievableAcceleration(double maxVoltage, double velocity) { + return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / 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. + */ + public double minAchievableAcceleration(double maxVoltage, double velocity) { + return maxAchievableAcceleration(-maxVoltage, velocity); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java index 68d4ecbfe1..9d2c57ed39 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java @@ -10,10 +10,11 @@ package edu.wpi.first.wpilibj.controller; /** * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */ +@SuppressWarnings("MemberName") public class SimpleMotorFeedforward { - private final double m_ks; - private final double m_kv; - private final double m_ka; + public final double ks; + public final double kv; + public final double ka; /** * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values @@ -24,9 +25,9 @@ public class SimpleMotorFeedforward { * @param ka The acceleration gain. */ public SimpleMotorFeedforward(double ks, double kv, double ka) { - m_ks = ks; - m_kv = kv; - m_ka = ka; + this.ks = ks; + this.kv = kv; + this.ka = ka; } /** @@ -48,9 +49,12 @@ public class SimpleMotorFeedforward { * @return The computed feedforward. */ public double calculate(double velocity, double acceleration) { - return m_ks * Math.signum(velocity) + m_kv * velocity + m_ka * acceleration; + return ks * Math.signum(velocity) + kv * velocity + ka * acceleration; } + // Rearranging the main equation from the calculate() method yields the + // formulas for the methods below: + /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to * be zero). @@ -61,4 +65,66 @@ public class SimpleMotorFeedforward { public double calculate(double velocity) { return calculate(velocity, 0); } + + /** + * 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. + */ + public double maxAchievableVelocity(double maxVoltage, double acceleration) { + // Assume max velocity is positive + return (maxVoltage - ks - acceleration * ka) / 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. + */ + public double minAchievableVelocity(double maxVoltage, double acceleration) { + // Assume min velocity is negative, ks flips sign + return (-maxVoltage + ks - acceleration * ka) / 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. + */ + public double maxAchievableAcceleration(double maxVoltage, double velocity) { + return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka; + } + + /** + * 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 minimum possible acceleration at the given velocity. + */ + public double minAchievableAcceleration(double maxVoltage, double velocity) { + return maxAchievableAcceleration(-maxVoltage, velocity); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java index 249cea7141..06fdd32e99 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java @@ -15,8 +15,9 @@ package edu.wpi.first.wpilibj.kinematics; * velocity components whereas forward kinematics converts left and right * component velocities into a linear and angular chassis speed. */ +@SuppressWarnings("MemberName") public class DifferentialDriveKinematics { - private final double m_trackWidthMeters; + public final double trackWidthMeters; /** * Constructs a differential drive kinematics object. @@ -27,7 +28,7 @@ public class DifferentialDriveKinematics { * measured value due to scrubbing effects. */ public DifferentialDriveKinematics(double trackWidthMeters) { - m_trackWidthMeters = trackWidthMeters; + this.trackWidthMeters = trackWidthMeters; } /** @@ -41,7 +42,7 @@ public class DifferentialDriveKinematics { return new ChassisSpeeds( (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0, (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) - / m_trackWidthMeters + / trackWidthMeters ); } @@ -55,9 +56,9 @@ public class DifferentialDriveKinematics { */ public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { return new DifferentialDriveWheelSpeeds( - chassisSpeeds.vxMetersPerSecond - m_trackWidthMeters / 2 + chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond, - chassisSpeeds.vxMetersPerSecond + m_trackWidthMeters / 2 + chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond ); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java index 274bdfbe5d..9a57720167 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java @@ -24,6 +24,7 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai /** * Constructs a differential drive dynamics constraint. * + * @param kinematics A kinematics component describing the drive geometry. * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. */ public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics, diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java new file mode 100644 index 0000000000..71432bfddf --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java @@ -0,0 +1,105 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory.constraint; + +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * 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. + */ +public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint { + private final SimpleMotorFeedforward m_feedforward; + private final DifferentialDriveKinematics m_kinematics; + private final double m_maxVoltage; + + /** + * 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. + */ + public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward, + DifferentialDriveKinematics kinematics, + double maxVoltage) { + m_feedforward = requireNonNullParam(feedforward, "feedforward", + "DifferentialDriveVoltageConstraint"); + m_kinematics = requireNonNullParam(kinematics, "kinematics", + "DifferentialDriveVoltageConstraint"); + m_maxVoltage = maxVoltage; + } + + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + return Double.POSITIVE_INFINITY; + } + + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + + var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0, + velocityMetersPerSecond + * curvatureRadPerMeter)); + + double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.rightMetersPerSecond); + double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond, + wheelSpeeds.rightMetersPerSecond); + + // Calculate maximum/minimum possible accelerations from motor dynamics + // and max/min wheel speeds + double maxWheelAcceleration = + m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed); + double minWheelAcceleration = + m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed); + + // Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius + // increased by half of the trackwidth T. Inner wheel has radius decreased + // by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so + // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2). + // Inner wheel is similar. + + // sgn(speed) term added to correctly account for which wheel is on + // outside of turn: + // If moving forward, max acceleration constraint corresponds to wheel on outside of turn + // If moving backward, max acceleration constraint corresponds to wheel on inside of turn + + double maxChassisAcceleration = + maxWheelAcceleration + / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) + * Math.signum(velocityMetersPerSecond) / 2); + double minChassisAcceleration = + minWheelAcceleration + / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) + * Math.signum(velocityMetersPerSecond) / 2); + + // Negate acceleration of wheel on inside of turn if center of turn is inside of wheelbase + if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) { + if (velocityMetersPerSecond > 0) { + minChassisAcceleration = -minChassisAcceleration; + } else { + maxChassisAcceleration = -maxChassisAcceleration; + } + } + + return new MinMax(minChassisAcceleration, maxChassisAcceleration); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java new file mode 100644 index 0000000000..f5c6ff32a4 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory; + +import java.util.Collections; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class DifferentialDriveVoltageConstraintTest { + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + @Test + void testDifferentialDriveVoltageConstraint() { + // Pick an unreasonably large kA to ensure the constraint has to do some work + var feedforward = new SimpleMotorFeedforward(1, 1, 3); + var kinematics = new DifferentialDriveKinematics(0.5); + double maxVoltage = 10; + var constraint = new DifferentialDriveVoltageConstraint(feedforward, + kinematics, + maxVoltage); + + Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory( + Collections.singletonList(constraint)); + + var duration = trajectory.getTotalTimeSeconds(); + var t = 0.0; + var dt = 0.02; + + while (t < duration) { + var point = trajectory.sample(t); + var chassisSpeeds = new ChassisSpeeds( + point.velocityMetersPerSecond, 0, + point.velocityMetersPerSecond * point.curvatureRadPerMeter + ); + + var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + + t += dt; + + // Not really a strictly-correct test as we're using the chassis accel instead of the + // wheel accel, but much easier than doing it "properly" and a reasonable check anyway + assertAll( + () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond, + point.accelerationMetersPerSecondSq) + <= maxVoltage + 0.05), + () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond, + point.accelerationMetersPerSecondSq) + >= -maxVoltage - 0.05), + () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond, + point.accelerationMetersPerSecondSq) + <= maxVoltage + 0.05), + () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond, + point.accelerationMetersPerSecondSq) + >= -maxVoltage - 0.05) + ); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java index 0d5397e4ad..7d47655e32 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java @@ -8,7 +8,6 @@ package edu.wpi.first.wpilibj.examples.ramsetecommand; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -63,11 +62,7 @@ public final class Constants { public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final DifferentialDriveKinematicsConstraint kAutoPathConstraints = - new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics, - kMaxSpeedMetersPerSecond); - - // Reasonable baseline values for a RAMSETE Controller in units of meters and seconds + // Reasonable baseline values for a RAMSETE follower in units of meters and seconds public static final double kRamseteB = 2; public static final double kRamseteZeta = 0.7; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index ccd1ae4429..c575e29d2f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -91,11 +92,23 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + + // Create a voltage constraint to ensure we don't accelerate too fast + var autoVoltageConstraint = + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward(ksVolts, + kvVoltSecondsPerMeter, + kaVoltSecondsSquaredPerMeter), + kDriveKinematics, + 10); + // Create config for trajectory TrajectoryConfig config = new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed - .setKinematics(kDriveKinematics); + .setKinematics(kDriveKinematics) + // Apply the voltage constraint + .addConstraint(autoVoltageConstraint); // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(