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

[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

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