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

Fix DifferentialDriveVoltageConstraint for tight turns w/ zero velocity (#2341)

Browse Source
This commit is contained in:
Oblarg
2020-02-15 15:36:16 -05:00
committed by GitHub
parent a226ad8509
commit 65eab93527
4 changed files with 130 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 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. */
@@ -82,20 +82,41 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
// 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);
// When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
// turning on its center) - we have to treat this as a special case, as it breaks
// the signum function. Both max and min acceleration are *reduced in magnitude*
// in this case.
double maxChassisAcceleration;
double minChassisAcceleration;
if (velocityMetersPerSecond == 0) {
maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
minChassisAcceleration =
minWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
} else {
maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
minChassisAcceleration =
minWheelAcceleration
/ (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
}
// When turning about a point inside of the wheelbase (i.e. radius less than half
// the trackwidth), the inner wheel's direction changes, but the magnitude remains
// the same. The formula above changes sign for the inner wheel when this happens.
// We can accurately account for this by simply negating the inner wheel.
// 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 {
} else if (velocityMetersPerSecond < 0) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}