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

Improved Drive docs and fix implementation bugs (#774)

Browse Source 
I also found some inconsistencies in MecanumDrive and KilloughDrive and fixed
them.

Drive now uses the NED axes convention. Therefore, the positive X axis points
ahead, the positive Y axis points to the right, and the positive Z axis points
down.

Translation in X assumes forward is positive. Translation in Y assumes right is
positive. Rotation rate assumes clockwise is positive. Angles are measured
clockwise from the positive X axis.

Based on the angle origin convention, DrivePolar() for both Mecanum and Killough
needed the normalization removed, sine used to compute the Y component, and
cosine used to compute the X component.

The vector rotation done in DriveCartesian() needs to rotate by a negative angle
instead of positive to undo the robot's rotation. RobotDrive assumed a clockwise
angle and sensors returned counter-clockwise angles, which is why it used a
positive angle for rotation.
This commit is contained in:
Tyler Veness
2017-11-26 18:36:51 -08:00
committed by Peter Johnson
parent 7a250a1b93
commit 34c44b7ae9
9 changed files with 409 additions and 254 deletions
Download Patch File Download Diff File Expand all files Collapse all files

134
wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp
View File

@@ -31,12 +31,13 @@ DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
* Note: Some drivers may prefer inverted rotation controls. This can be done by
* negating the value passed for rotation.
*
* @param y The value to use for forwards/backwards. [-1.0..1.0]
* @param rotation The value to use for the rotation right/left.
* [-1.0..1.0]
* @param xSpeed The speed at which the robot should drive along the X
* axis [-1.0..1.0]. Forward is negative.
* @param zRotation The rotation rate of the robot around the Z axis
* [-1.0..1.0]. Clockwise is positive.
* @param squaredInputs If set, decreases the input sensitivity at low speeds.
*/
void DifferentialDrive::ArcadeDrive(double y, double rotation,
void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
bool squaredInputs) {
static bool reported = false;
if (!reported) {
@@ -45,41 +46,42 @@ void DifferentialDrive::ArcadeDrive(double y, double rotation,
reported = true;
}
y = Limit(y);
y = ApplyDeadband(y, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
rotation = Limit(rotation);
rotation = ApplyDeadband(rotation, m_deadband);
zRotation = Limit(zRotation);
zRotation = ApplyDeadband(zRotation, m_deadband);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squaredInputs) {
y = std::copysign(y * y, y);
rotation = std::copysign(rotation * rotation, rotation);
xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
zRotation = std::copysign(zRotation * zRotation, zRotation);
}
double leftMotorOutput;
double rightMotorOutput;
double maxInput = std::copysign(std::max(std::abs(y), std::abs(rotation)), y);
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
if (y > 0.0) {
if (xSpeed >= 0.0) {
// First quadrant, else second quadrant
if (rotation > 0.0) {
if (zRotation >= 0.0) {
leftMotorOutput = maxInput;
rightMotorOutput = y - rotation;
rightMotorOutput = xSpeed - zRotation;
} else {
leftMotorOutput = y + rotation;
leftMotorOutput = xSpeed + zRotation;
rightMotorOutput = maxInput;
}
} else {
// Third quadrant, else fourth quadrant
if (rotation > 0.0) {
leftMotorOutput = y + rotation;
if (zRotation >= 0.0) {
leftMotorOutput = xSpeed + zRotation;
rightMotorOutput = maxInput;
} else {
leftMotorOutput = maxInput;
rightMotorOutput = y - rotation;
rightMotorOutput = xSpeed - zRotation;
}
}
@@ -97,12 +99,14 @@ void DifferentialDrive::ArcadeDrive(double y, double rotation,
* speeds. Also handles the robot's quick turn functionality - "quick turn"
* overrides constant-curvature turning for turn-in-place maneuvers.
*
* @param y The value to use for forwards/backwards. [-1.0..1.0]
* @param rotation The value to use for the rotation right/left. [-1.0..1.0]
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param isQuickTurn If set, overrides constant-curvature turning for
* turn-in-place maneuvers.
*/
void DifferentialDrive::CurvatureDrive(double y, double rotation,
void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
bool isQuickTurn) {
static bool reported = false;
if (!reported) {
@@ -111,26 +115,25 @@ void DifferentialDrive::CurvatureDrive(double y, double rotation,
reported = true;
}
y = Limit(y);
y = ApplyDeadband(y, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
rotation = Limit(rotation);
rotation = ApplyDeadband(rotation, m_deadband);
zRotation = Limit(zRotation);
zRotation = ApplyDeadband(zRotation, m_deadband);
double angularPower;
bool overPower;
if (isQuickTurn) {
if (std::abs(y) < 0.2) {
constexpr double alpha = 0.1;
m_quickStopAccumulator =
(1 - alpha) * m_quickStopAccumulator + alpha * Limit(rotation) * 2;
if (std::abs(xSpeed) < m_quickStopThreshold) {
m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator +
m_quickStopAlpha * Limit(zRotation) * 2;
}
overPower = true;
angularPower = rotation;
angularPower = zRotation;
} else {
overPower = false;
angularPower = std::abs(y) * rotation - m_quickStopAccumulator;
angularPower = std::abs(xSpeed) * zRotation - m_quickStopAccumulator;
if (m_quickStopAccumulator > 1) {
m_quickStopAccumulator -= 1;
@@ -141,8 +144,8 @@ void DifferentialDrive::CurvatureDrive(double y, double rotation,
}
}
double leftMotorOutput = y + angularPower;
double rightMotorOutput = y - angularPower;
double leftMotorOutput = xSpeed + angularPower;
double rightMotorOutput = xSpeed - angularPower;
// If rotation is overpowered, reduce both outputs to within acceptable range
if (overPower) {
@@ -170,11 +173,13 @@ void DifferentialDrive::CurvatureDrive(double y, double rotation,
/**
* Tank drive method for differential drive platform.
*
* @param left The value to use for left side motors. [-1.0..1.0]
* @param right The value to use for right side motors. [-1.0..1.0]
* @param leftSpeed The robot left side's speed along the X axis
* [-1.0..1.0]. Forward is positive.
* @param rightSpeed The robot right side's speed along the X axis
* [-1.0..1.0]. Forward is positive.
* @param squaredInputs If set, decreases the input sensitivity at low speeds.
*/
void DifferentialDrive::TankDrive(double left, double right,
void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
bool squaredInputs) {
static bool reported = false;
if (!reported) {
@@ -183,25 +188,58 @@ void DifferentialDrive::TankDrive(double left, double right,
reported = true;
}
left = Limit(left);
left = ApplyDeadband(left, m_deadband);
leftSpeed = Limit(leftSpeed);
leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
right = Limit(right);
right = ApplyDeadband(right, m_deadband);
rightSpeed = Limit(rightSpeed);
rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squaredInputs) {
left = std::copysign(left * left, left);
right = std::copysign(right * right, right);
leftSpeed = std::copysign(leftSpeed * leftSpeed, leftSpeed);
rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
}
m_leftMotor.Set(left * m_maxOutput);
m_rightMotor.Set(-right * m_maxOutput);
m_leftMotor.Set(leftSpeed * m_maxOutput);
m_rightMotor.Set(-rightSpeed * m_maxOutput);
m_safetyHelper.Feed();
}
/**
* Sets the QuickStop speed threshold in curvature drive.
*
* QuickStop compensates for the robot's moment of inertia when stopping after a
* QuickTurn.
*
* While QuickTurn is enabled, the QuickStop accumulator takes on the rotation
* rate value outputted by the low-pass filter when the robot's speed along the
* X axis is below the threshold. When QuickTurn is disabled, the accumulator's
* value is applied against the computed angular power request to slow the
* robot's rotation.
*
* @param threshold X speed below which quick stop accumulator will receive
* rotation rate values [0..1.0].
*/
void DifferentialDrive::SetQuickStopThreshold(double threshold) {
m_quickStopThreshold = threshold;
}
/**
* Sets the low-pass filter gain for QuickStop in curvature drive.
*
* The low-pass filter filters incoming rotation rate commands to smooth out
* high frequency changes.
*
* @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower
* output changes. Values between 1.0 and 2.0 result in output
* oscillation. Values below 0.0 and above 2.0 are unstable.
*/
void DifferentialDrive::SetQuickStopAlpha(double alpha) {
m_quickStopAlpha = alpha;
}
void DifferentialDrive::StopMotor() {
m_leftMotor.StopMotor();
m_rightMotor.StopMotor();

75
wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp
View File

@@ -21,9 +21,8 @@ constexpr double kPi = 3.14159265358979323846;
/**
* Construct a Killough drive with the given motors and default motor angles.
*
* The default motor angles are 120, 60, and 270 degrees for the left, right,
* and back motors respectively, which make the wheels on each corner parallel
* to their respective opposite sides.
* The default motor angles make the wheels on each corner parallel to their
* respective opposite sides.
*
* If a motor needs to be inverted, do so before passing it in.
*
@@ -34,13 +33,13 @@ constexpr double kPi = 3.14159265358979323846;
KilloughDrive::KilloughDrive(SpeedController& leftMotor,
SpeedController& rightMotor,
SpeedController& backMotor)
: KilloughDrive(leftMotor, rightMotor, backMotor, 120.0, 60.0, 270.0) {}
: KilloughDrive(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle,
kDefaultRightMotorAngle, kDefaultBackMotorAngle) {}
/**
* Construct a Killough drive with the given motors.
*
* Angles are measured in counter-clockwise degrees where zero degrees is
* straight ahead.
* Angles are measured in degrees clockwise from the positive X axis.
*
* @param leftMotor The motor on the left corner.
* @param rightMotor The motor on the right corner.
@@ -68,37 +67,40 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor,
/**
* Drive method for Killough platform.
*
* @param x The speed that the robot should drive in the X direction.
* [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely
* independent of the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to
* implement field-oriented controls.
* Angles are measured clockwise from the positive X axis. The robot's speed is
* independent from its angle or rotation rate.
*
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is
* positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param gyroAngle The current angle reading from the gyro in degrees around
* the Z axis. Use this to implement field-oriented controls.
*/
void KilloughDrive::DriveCartesian(double x, double y, double rotation,
double gyroAngle) {
void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
double zRotation, double gyroAngle) {
if (!reported) {
// HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
// HALUsageReporting::kRobotDrive_KilloughCartesian);
reported = true;
}
x = Limit(x);
x = ApplyDeadband(x, m_deadband);
ySpeed = Limit(ySpeed);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
y = Limit(y);
y = ApplyDeadband(y, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
Vector2d input{x, y};
input.Rotate(gyroAngle);
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
double wheelSpeeds[3];
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + rotation;
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + rotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + rotation;
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
Normalize(wheelSpeeds);
@@ -112,13 +114,15 @@ void KilloughDrive::DriveCartesian(double x, double y, double rotation,
/**
* Drive method for Killough platform.
*
* @param magnitude The speed that the robot should drive in a given direction.
* [-1.0..1.0]
* @param angle The direction the robot should drive in degrees. 0.0 is
* straight ahead. The direction and maginitude are independent
* of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely
* independent of the magnitude or direction. [-1.0..1.0]
* Angles are measured clockwise from the positive X axis. The robot's speed is
* independent from its angle or rotation rate.
*
* @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is
* positive.
* @param angle The angle around the Z axis at which the robot drives in
* degrees [-180..180].
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
*/
void KilloughDrive::DrivePolar(double magnitude, double angle,
double rotation) {
@@ -128,11 +132,8 @@ void KilloughDrive::DrivePolar(double magnitude, double angle,
reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * std::sqrt(2.0);
DriveCartesian(magnitude * std::cos(angle * (kPi / 180.0)),
magnitude * std::sin(angle * (kPi / 180.0)), rotation, 0.0);
DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
magnitude * std::cos(angle * (kPi / 180.0)), rotation, 0.0);
}
void KilloughDrive::StopMotor() {

69
wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp
View File

@@ -36,38 +36,41 @@ MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
/**
* Drive method for Mecanum platform.
*
* @param x The speed that the robot should drive in the X direction.
* [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely
* independent of the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to
* implement field-oriented controls.
* Angles are measured clockwise from the positive X axis. The robot's speed is
* independent from its angle or rotation rate.
*
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is
* positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param gyroAngle The current angle reading from the gyro in degrees around
* the Z axis. Use this to implement field-oriented controls.
*/
void MecanumDrive::DriveCartesian(double x, double y, double rotation,
double gyroAngle) {
void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
double zRotation, double gyroAngle) {
if (!reported) {
HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
HALUsageReporting::kRobotDrive_MecanumCartesian);
reported = true;
}
x = Limit(x);
x = ApplyDeadband(x, m_deadband);
ySpeed = Limit(ySpeed);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
y = Limit(y);
y = ApplyDeadband(y, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
Vector2d input{x, y};
input.Rotate(gyroAngle);
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
double wheelSpeeds[4];
wheelSpeeds[kFrontLeft] = input.x + input.y + rotation;
wheelSpeeds[kFrontRight] = input.x - input.y + rotation;
wheelSpeeds[kRearLeft] = -input.x + input.y + rotation;
wheelSpeeds[kRearRight] = -input.x - input.y + rotation;
wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
wheelSpeeds[kFrontRight] = input.x - input.y + zRotation;
wheelSpeeds[kRearLeft] = -input.x + input.y + zRotation;
wheelSpeeds[kRearRight] = -input.x - input.y + zRotation;
Normalize(wheelSpeeds);
@@ -82,26 +85,26 @@ void MecanumDrive::DriveCartesian(double x, double y, double rotation,
/**
* Drive method for Mecanum platform.
*
* @param magnitude The speed that the robot should drive in a given direction.
* [-1.0..1.0]
* @param angle The direction the robot should drive in degrees. 0.0 is
* straight ahead. The direction and maginitude are independent
* of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely
* independent of the magnitude or direction. [-1.0..1.0]
* Angles are measured clockwise from the positive X axis. The robot's speed is
* independent from its angle or rotation rate.
*
* @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is
* positive.
* @param angle The angle around the Z axis at which the robot drives in
* degrees [-180..180].
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
*/
void MecanumDrive::DrivePolar(double magnitude, double angle, double rotation) {
void MecanumDrive::DrivePolar(double magnitude, double angle,
double zRotation) {
if (!reported) {
HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
HALUsageReporting::kRobotDrive_MecanumPolar);
reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * std::sqrt(2.0);
DriveCartesian(magnitude * std::cos(angle * (kPi / 180.0)),
magnitude * std::sin(angle * (kPi / 180.0)), rotation, 0.0);
DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
}
void MecanumDrive::StopMotor() {