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

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

29
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
View File

@@ -103,6 +103,7 @@ public class DifferentialDrive extends RobotDriveBase {
private double m_quickStopThreshold = kDefaultQuickStopThreshold;
private double m_quickStopAlpha = kDefaultQuickStopAlpha;
private double m_quickStopAccumulator = 0.0;
private double m_rightSideInvertMultiplier = -1.0;
private boolean m_reported = false;
/**
@@ -187,7 +188,7 @@ public class DifferentialDrive extends RobotDriveBase {
}
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();
}
@@ -270,7 +271,7 @@ public class DifferentialDrive extends RobotDriveBase {
}
m_leftMotor.set(leftMotorOutput * m_maxOutput);
m_rightMotor.set(-rightMotorOutput * m_maxOutput);
m_rightMotor.set(rightMotorOutput * m_maxOutput * m_rightSideInvertMultiplier);
m_safetyHelper.feed();
}
@@ -317,7 +318,7 @@ public class DifferentialDrive extends RobotDriveBase {
}
m_leftMotor.set(leftSpeed * m_maxOutput);
m_rightMotor.set(-rightSpeed * m_maxOutput);
m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
m_safetyHelper.feed();
}
@@ -353,6 +354,24 @@ public class DifferentialDrive extends RobotDriveBase {
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
*/
public boolean isRightSideInverted() {
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
*/
public void setRightSideInverted(boolean rightSideInverted) {
m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
}
@Override
public void stopMotor() {
m_leftMotor.stopMotor();
@@ -371,7 +390,7 @@ public class DifferentialDrive extends RobotDriveBase {
builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
builder.addDoubleProperty(
"Right Motor Speed",
() -> -m_rightMotor.get(),
x -> m_rightMotor.set(-x));
() -> m_rightMotor.get() * m_rightSideInvertMultiplier,
x -> m_rightMotor.set(x * m_rightSideInvertMultiplier));
}
}

45
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
View File

@@ -64,6 +64,7 @@ public class MecanumDrive extends RobotDriveBase {
private SpeedController m_frontRightMotor;
private SpeedController m_rearRightMotor;
private double m_rightSideInvertMultiplier = -1.0;
private boolean m_reported = false;
/**
@@ -134,16 +135,18 @@ public class MecanumDrive extends RobotDriveBase {
double[] wheelSpeeds = new double[4];
wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y + zRotation;
wheelSpeeds[MotorType.kFrontRight.value] = -input.x + input.y - zRotation;
wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
wheelSpeeds[MotorType.kRearRight.value] = -input.x - input.y + zRotation;
wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
normalize(wheelSpeeds);
m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput
* m_rightSideInvertMultiplier);
m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput
* m_rightSideInvertMultiplier);
m_safetyHelper.feed();
}
@@ -170,6 +173,24 @@ public class MecanumDrive extends RobotDriveBase {
magnitude * Math.cos(angle * (Math.PI / 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
*/
public boolean isRightSideInverted() {
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
*/
public void setRightSideInverted(boolean rightSideInverted) {
m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
}
@Override
public void stopMotor() {
m_frontLeftMotor.stopMotor();
@@ -187,13 +208,17 @@ public class MecanumDrive extends RobotDriveBase {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("MecanumDrive");
builder.addDoubleProperty("Front Left Motor Speed", m_frontLeftMotor::get,
builder.addDoubleProperty("Front Left Motor Speed",
m_frontLeftMotor::get,
m_frontLeftMotor::set);
builder.addDoubleProperty("Front Right Motor Speed", () -> -m_frontRightMotor.get(),
value -> m_frontRightMotor.set(-value));
builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get,
builder.addDoubleProperty("Front Right Motor Speed",
() -> m_frontRightMotor.get() * m_rightSideInvertMultiplier,
value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier));
builder.addDoubleProperty("Rear Left Motor Speed",
m_rearLeftMotor::get,
m_rearLeftMotor::set);
builder.addDoubleProperty("Rear Right Motor Speed", () -> -m_rearRightMotor.get(),
value -> m_rearRightMotor.set(-value));
builder.addDoubleProperty("Rear Right Motor Speed",
() -> m_rearRightMotor.get() * m_rightSideInvertMultiplier,
value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier));
}
}