Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21: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); });
}

42
wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
View File

@@ -60,26 +60,15 @@ void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
reported = true;
}
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
auto [left, right, back] =
DriveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
double wheelSpeeds[3];
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
Normalize(wheelSpeeds);
m_leftMotor->Set(wheelSpeeds[kLeft] * m_maxOutput);
m_rightMotor->Set(wheelSpeeds[kRight] * m_maxOutput);
m_backMotor->Set(wheelSpeeds[kBack] * m_maxOutput);
m_leftMotor->Set(left * m_maxOutput);
m_rightMotor->Set(right * m_maxOutput);
m_backMotor->Set(back * m_maxOutput);
Feed();
}
@@ -97,6 +86,27 @@ void KilloughDrive::DrivePolar(double magnitude, double angle,
zRotation, 0.0);
}
KilloughDrive::WheelSpeeds KilloughDrive::DriveCartesianIK(double ySpeed,
double xSpeed,
double zRotation,
double gyroAngle) {
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
// Compensate for gyro angle.
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
double wheelSpeeds[3];
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
Normalize(wheelSpeeds);
return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]};
}
void KilloughDrive::StopMotor() {
m_leftMotor->StopMotor();
m_rightMotor->StopMotor();

75
wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
View File

@@ -51,30 +51,16 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
reported = true;
}
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
auto [frontLeft, frontRight, rearLeft, rearRight] =
DriveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
double wheelSpeeds[4];
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);
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
m_frontRightMotor->Set(wheelSpeeds[kFrontRight] * m_maxOutput *
m_rightSideInvertMultiplier);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeft] * m_maxOutput);
m_rearRightMotor->Set(wheelSpeeds[kRearRight] * m_maxOutput *
m_rightSideInvertMultiplier);
m_frontLeftMotor->Set(frontLeft * m_maxOutput);
m_frontRightMotor->Set(frontRight * m_maxOutput);
m_rearLeftMotor->Set(rearLeft * m_maxOutput);
m_rearRightMotor->Set(rearRight * m_maxOutput);
Feed();
}
@@ -87,19 +73,11 @@ void MecanumDrive::DrivePolar(double magnitude, double angle,
reported = true;
}
DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
DriveCartesian(magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
zRotation, 0.0);
}
bool MecanumDrive::IsRightSideInverted() const {
return m_rightSideInvertMultiplier == -1.0;
}
void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
}
void MecanumDrive::StopMotor() {
m_frontLeftMotor->StopMotor();
m_frontRightMotor->StopMotor();
@@ -108,6 +86,29 @@ void MecanumDrive::StopMotor() {
Feed();
}
MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double ySpeed,
double xSpeed,
double zRotation,
double gyroAngle) {
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
// Compensate for gyro angle.
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
double wheelSpeeds[4];
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);
return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};
}
void MecanumDrive::GetDescription(wpi::raw_ostream& desc) const {
desc << "MecanumDrive";
}
@@ -120,18 +121,12 @@ void MecanumDrive::InitSendable(SendableBuilder& builder) {
"Front Left Motor Speed", [=]() { return m_frontLeftMotor->Get(); },
[=](double value) { m_frontLeftMotor->Set(value); });
builder.AddDoubleProperty(
"Front Right Motor Speed",
[=]() { return m_frontRightMotor->Get() * m_rightSideInvertMultiplier; },
[=](double value) {
m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
});
"Front Right Motor Speed", [=] { return m_frontRightMotor->Get(); },
[=](double value) { m_frontRightMotor->Set(value); });
builder.AddDoubleProperty(
"Rear Left Motor Speed", [=]() { return m_rearLeftMotor->Get(); },
[=](double value) { m_rearLeftMotor->Set(value); });
builder.AddDoubleProperty(
"Rear Right Motor Speed",
[=]() { return m_rearRightMotor->Get() * m_rightSideInvertMultiplier; },
[=](double value) {
m_rearRightMotor->Set(value * m_rightSideInvertMultiplier);
});
"Rear Right Motor Speed", [=] { return m_rearRightMotor->Get(); },
[=](double value) { m_rearRightMotor->Set(value); });
}