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

92
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
View File

@@ -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);
}
}

84
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
View File

@@ -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);
}
}

80
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
View File

@@ -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);
}
}

11
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
View File

@@ -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
);
}

1
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
View File

@@ -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,

105
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java Normal file
View File

@@ -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);
}
}