diff --git a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp index 4f8802dfd6..b8cab7ed83 100644 --- a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp @@ -7,6 +7,7 @@ #include "Drive/DifferentialDrive.h" +#include #include #include @@ -171,6 +172,14 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation, } } + // 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); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 74ff519c4e..79b011f69c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -261,6 +261,13 @@ public class DifferentialDrive extends RobotDriveBase { } } + // Normalize the wheel speeds + double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput)); + if (maxMagnitude > 1.0) { + leftMotorOutput /= maxMagnitude; + rightMotorOutput /= maxMagnitude; + } + m_leftMotor.set(leftMotorOutput * m_maxOutput); m_rightMotor.set(-rightMotorOutput * m_maxOutput);