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

Add setting to invert the right side of the drive (#1045)

Browse Source
This commit is contained in:
Austin Shalit
2018-05-19 04:22:20 -04:00
committed by Peter Johnson
parent 73439d8213
commit 17401e10f0
6 changed files with 135 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -94,7 +94,8 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
}
m_leftMotor.Set(Limit(leftMotorOutput) * m_maxOutput);
m_rightMotor.Set(-Limit(rightMotorOutput) * m_maxOutput);
m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
m_rightSideInvertMultiplier);
m_safetyHelper.Feed();
}
@@ -181,7 +182,8 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
}
m_leftMotor.Set(leftMotorOutput * m_maxOutput);
m_rightMotor.Set(-rightMotorOutput * m_maxOutput);
m_rightMotor.Set(rightMotorOutput * m_maxOutput *
m_rightSideInvertMultiplier);
m_safetyHelper.Feed();
}
@@ -218,7 +220,7 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
}
m_leftMotor.Set(leftSpeed * m_maxOutput);
m_rightMotor.Set(-rightSpeed * m_maxOutput);
m_rightMotor.Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
m_safetyHelper.Feed();
}
@@ -256,6 +258,26 @@ void DifferentialDrive::SetQuickStopAlpha(double alpha) {
m_quickStopAlpha = alpha;
}
/**
* Gets if the power sent to the right side of the drivetrain is multipled by
* -1.
*
* @return true if the right side is inverted
*/
bool DifferentialDrive::IsRightSideInverted() const {
return m_rightSideInvertMultiplier == -1.0;
}
/**
* Sets if the power sent to the right side of the drivetrain should be
* multipled by -1.
*
* @param rightSideInverted true if right side power should be multipled by -1
*/
void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
}
void DifferentialDrive::StopMotor() {
m_leftMotor.StopMotor();
m_rightMotor.StopMotor();
@@ -271,7 +293,10 @@ void DifferentialDrive::InitSendable(SendableBuilder& builder) {
builder.AddDoubleProperty("Left Motor Speed",
[=]() { return m_leftMotor.Get(); },
[=](double value) { m_leftMotor.Set(value); });
builder.AddDoubleProperty("Right Motor Speed",
[=]() { return -m_rightMotor.Get(); },
[=](double value) { m_rightMotor.Set(-value); });
builder.AddDoubleProperty(
"Right Motor Speed",
[=]() { return m_rightMotor.Get() * m_rightSideInvertMultiplier; },
[=](double value) {
m_rightMotor.Set(value * m_rightSideInvertMultiplier);
});
}

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

@@ -77,16 +77,18 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
double wheelSpeeds[4];
wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
wheelSpeeds[kFrontRight] = 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;
wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
Normalize(wheelSpeeds);
m_frontLeftMotor.Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * 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_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput *
m_rightSideInvertMultiplier);
m_safetyHelper.Feed();
}
@@ -116,6 +118,26 @@ void MecanumDrive::DrivePolar(double magnitude, double angle,
magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
}
/**
* Gets if the power sent to the right side of the drivetrain is multipled by
* -1.
*
* @return true if the right side is inverted
*/
bool MecanumDrive::IsRightSideInverted() const {
return m_rightSideInvertMultiplier == -1.0;
}
/**
* Sets if the power sent to the right side of the drivetrain should be
* multipled by -1.
*
* @param rightSideInverted true if right side power should be multipled by -1
*/
void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
}
void MecanumDrive::StopMotor() {
m_frontLeftMotor.StopMotor();
m_frontRightMotor.StopMotor();
@@ -134,12 +156,18 @@ void MecanumDrive::InitSendable(SendableBuilder& builder) {
[=]() { return m_frontLeftMotor.Get(); },
[=](double value) { m_frontLeftMotor.Set(value); });
builder.AddDoubleProperty(
"Front Right Motor Speed", [=]() { return -m_frontRightMotor.Get(); },
[=](double value) { m_frontRightMotor.Set(-value); });
"Front Right Motor Speed",
[=]() { return m_frontRightMotor.Get() * m_rightSideInvertMultiplier; },
[=](double value) {
m_frontRightMotor.Set(value * m_rightSideInvertMultiplier);
});
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(); },
[=](double value) { m_rearRightMotor.Set(-value); });
"Rear Right Motor Speed",
[=]() { return m_rearRightMotor.Get() * m_rightSideInvertMultiplier; },
[=](double value) {
m_rearRightMotor.Set(value * m_rightSideInvertMultiplier);
});
}