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

[wpilib] DifferentialDrive: Remove right side inversion (#3340)

Browse Source 
Also refactor drive inverse kinematics into separate functions.
This allows composing them with operations separate from the drive
class.
This commit is contained in:
Tyler Veness
2021-05-21 22:34:16 -07:00
committed by GitHub
parent 8dd8d4d2d4
commit 0768c39036
17 changed files with 1533 additions and 478 deletions
Download Patch File Download Diff File Expand all files Collapse all files

221
wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
View File

@@ -43,54 +43,19 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
reported = true;
}
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
zRotation = std::clamp(zRotation, -1.0, 1.0);
zRotation = ApplyDeadband(zRotation, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
zRotation = std::copysign(zRotation * zRotation, zRotation);
}
auto [left, right] = ArcadeDriveIK(xSpeed, zRotation, squareInputs);
double leftMotorOutput;
double rightMotorOutput;
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
if (xSpeed >= 0.0) {
// First quadrant, else second quadrant
if (zRotation >= 0.0) {
leftMotorOutput = maxInput;
rightMotorOutput = xSpeed - zRotation;
} else {
leftMotorOutput = xSpeed + zRotation;
rightMotorOutput = maxInput;
}
} else {
// Third quadrant, else fourth quadrant
if (zRotation >= 0.0) {
leftMotorOutput = xSpeed + zRotation;
rightMotorOutput = maxInput;
} else {
leftMotorOutput = maxInput;
rightMotorOutput = xSpeed - zRotation;
}
}
m_leftMotor->Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
m_rightMotor->Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
m_leftMotor->Set(left);
m_rightMotor->Set(right);
Feed();
}
void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
bool isQuickTurn) {
bool allowTurnInPlace) {
static bool reported = false;
if (!reported) {
HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
@@ -98,67 +63,13 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
reported = true;
}
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
zRotation = std::clamp(zRotation, -1.0, 1.0);
zRotation = ApplyDeadband(zRotation, m_deadband);
double angularPower;
bool overPower;
auto [left, right] = CurvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
if (isQuickTurn) {
if (std::abs(xSpeed) < m_quickStopThreshold) {
m_quickStopAccumulator =
(1 - m_quickStopAlpha) * m_quickStopAccumulator +
m_quickStopAlpha * std::clamp(zRotation, -1.0, 1.0) * 2;
}
overPower = true;
angularPower = zRotation;
} else {
overPower = false;
angularPower = std::abs(xSpeed) * zRotation - m_quickStopAccumulator;
if (m_quickStopAccumulator > 1) {
m_quickStopAccumulator -= 1;
} else if (m_quickStopAccumulator < -1) {
m_quickStopAccumulator += 1;
} else {
m_quickStopAccumulator = 0.0;
}
}
double leftMotorOutput = xSpeed + angularPower;
double rightMotorOutput = xSpeed - angularPower;
// If rotation is overpowered, reduce both outputs to within acceptable range
if (overPower) {
if (leftMotorOutput > 1.0) {
rightMotorOutput -= leftMotorOutput - 1.0;
leftMotorOutput = 1.0;
} else if (rightMotorOutput > 1.0) {
leftMotorOutput -= rightMotorOutput - 1.0;
rightMotorOutput = 1.0;
} else if (leftMotorOutput < -1.0) {
rightMotorOutput -= leftMotorOutput + 1.0;
leftMotorOutput = -1.0;
} else if (rightMotorOutput < -1.0) {
leftMotorOutput -= rightMotorOutput + 1.0;
rightMotorOutput = -1.0;
}
}
// Normalize the wheel speeds
double maxMagnitude =
std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
if (maxMagnitude > 1.0) {
leftMotorOutput /= maxMagnitude;
rightMotorOutput /= maxMagnitude;
}
m_leftMotor->Set(leftMotorOutput * m_maxOutput);
m_rightMotor->Set(rightMotorOutput * m_maxOutput *
m_rightSideInvertMultiplier);
m_leftMotor->Set(left * m_maxOutput);
m_rightMotor->Set(right * m_maxOutput);
Feed();
}
@@ -172,12 +83,96 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
reported = true;
}
leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
auto [left, right] = TankDriveIK(leftSpeed, rightSpeed, squareInputs);
m_leftMotor->Set(left * m_maxOutput);
m_rightMotor->Set(right * m_maxOutput);
Feed();
}
DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
double xSpeed, double zRotation, bool squareInputs) {
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
zRotation = std::clamp(zRotation, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
zRotation = std::copysign(zRotation * zRotation, zRotation);
}
double leftSpeed;
double rightSpeed;
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
if (xSpeed >= 0.0) {
// First quadrant, else second quadrant
if (zRotation >= 0.0) {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
} else {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
}
} else {
// Third quadrant, else fourth quadrant
if (zRotation >= 0.0) {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
} else {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
}
}
// Normalize the wheel speeds
double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
if (maxMagnitude > 1.0) {
leftSpeed /= maxMagnitude;
rightSpeed /= maxMagnitude;
}
return {leftSpeed, rightSpeed};
}
DifferentialDrive::WheelSpeeds DifferentialDrive::CurvatureDriveIK(
double xSpeed, double zRotation, bool allowTurnInPlace) {
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
zRotation = std::clamp(zRotation, -1.0, 1.0);
double leftSpeed = 0.0;
double rightSpeed = 0.0;
if (allowTurnInPlace) {
leftSpeed = xSpeed + zRotation;
rightSpeed = xSpeed - zRotation;
} else {
leftSpeed = xSpeed + std::abs(xSpeed) * zRotation;
rightSpeed = xSpeed - std::abs(xSpeed) * zRotation;
}
// Normalize wheel speeds
double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
if (maxMagnitude > 1.0) {
leftSpeed /= maxMagnitude;
rightSpeed /= maxMagnitude;
}
return {leftSpeed, rightSpeed};
}
DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK(
double leftSpeed, double rightSpeed, bool squareInputs) {
leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
@@ -185,26 +180,7 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
}
m_leftMotor->Set(leftSpeed * m_maxOutput);
m_rightMotor->Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
Feed();
}
void DifferentialDrive::SetQuickStopThreshold(double threshold) {
m_quickStopThreshold = threshold;
}
void DifferentialDrive::SetQuickStopAlpha(double alpha) {
m_quickStopAlpha = alpha;
}
bool DifferentialDrive::IsRightSideInverted() const {
return m_rightSideInvertMultiplier == -1.0;
}
void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
return {leftSpeed, rightSpeed};
}
void DifferentialDrive::StopMotor() {
@@ -225,9 +201,6 @@ void DifferentialDrive::InitSendable(SendableBuilder& builder) {
"Left Motor Speed", [=]() { return m_leftMotor->Get(); },
[=](double value) { m_leftMotor->Set(value); });
builder.AddDoubleProperty(
"Right Motor Speed",
[=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },
[=](double value) {
m_rightMotor->Set(value * m_rightSideInvertMultiplier);
});
"Right Motor Speed", [=]() { return m_rightMotor->Get(); },
[=](double value) { m_rightMotor->Set(value); });
}