diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp index 59e3622dbe..7def42a5c7 100644 --- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp @@ -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); }); } diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp index 7cdf145d8f..7863eab91b 100644 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp @@ -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(); diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index d9886b66f5..3f4d2c9ea9 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -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); }); } diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h index 57a125e558..d68f4f306e 100644 --- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h @@ -95,23 +95,15 @@ class SpeedController; * Inputs smaller then 0.02 will be set to 0, and larger values will be scaled * so that the full range is still used. This deadband value can be changed * with SetDeadband(). - * - *

RobotDrive porting guide: - *
TankDrive(double, double, bool) is equivalent to - * RobotDrive's TankDrive(double, double, bool) if a deadband of 0 is used. - *
ArcadeDrive(double, double, bool) is equivalent to - * RobotDrive's ArcadeDrive(double, double, bool) if a deadband of 0 is used - * and the the rotation input is inverted eg ArcadeDrive(y, -rotation, false) - *
CurvatureDrive(double, double, bool) is similar in concept to - * RobotDrive's Drive(double, double) with the addition of a quick turn - * mode. However, it is not designed to give exactly the same response. */ class DifferentialDrive : public RobotDriveBase, public Sendable, public SendableHelper { public: - static constexpr double kDefaultQuickStopThreshold = 0.2; - static constexpr double kDefaultQuickStopAlpha = 0.1; + struct WheelSpeeds { + double left = 0.0; + double right = 0.0; + }; /** * Construct a DifferentialDrive. @@ -148,14 +140,14 @@ class DifferentialDrive : public RobotDriveBase, * high speeds. Also handles the robot's quick turn functionality - "quick * turn" overrides constant-curvature turning for turn-in-place maneuvers. * - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward - * is positive. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. - * Clockwise is positive. - * @param isQuickTurn If set, overrides constant-curvature turning for - * turn-in-place maneuvers. + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. + * Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis + * [-1.0..1.0]. Clockwise is positive. + * @param allowTurnInPlace If set, overrides constant-curvature turning for + * turn-in-place maneuvers. */ - void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn); + void CurvatureDrive(double xSpeed, double zRotation, bool allowTurnInPlace); /** * Tank drive method for differential drive platform. @@ -169,51 +161,49 @@ class DifferentialDrive : public RobotDriveBase, void TankDrive(double leftSpeed, double rightSpeed, bool squareInputs = true); /** - * Sets the QuickStop speed threshold in curvature drive. + * Arcade drive inverse kinematics for differential drive platform. * - * QuickStop compensates for the robot's moment of inertia when stopping after - * a QuickTurn. + * Note: Some drivers may prefer inverted rotation controls. This can be done + * by negating the value passed for rotation. * - * While QuickTurn is enabled, the QuickStop accumulator takes on the rotation - * rate value outputted by the low-pass filter when the robot's speed along - * the X axis is below the threshold. When QuickTurn is disabled, the - * accumulator's value is applied against the computed angular power request - * to slow the robot's rotation. - * - * @param threshold X speed below which quick stop accumulator will receive - * rotation rate values [0..1.0]. + * @param xSpeed The speed at which the robot should drive along the X + * axis [-1.0..1.0]. Forward is positive. + * @param zRotation The rotation rate of the robot around the Z axis + * [-1.0..1.0]. Clockwise is positive. + * @param squareInputs If set, decreases the input sensitivity at low speeds. */ - void SetQuickStopThreshold(double threshold); + static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation, + bool squareInputs = true); /** - * Sets the low-pass filter gain for QuickStop in curvature drive. + * Curvature drive inverse kinematics for differential drive platform. * - * The low-pass filter filters incoming rotation rate commands to smooth out - * high frequency changes. + * The rotation argument controls the curvature of the robot's path rather + * than its rate of heading change. This makes the robot more controllable at + * high speeds. Also handles the robot's quick turn functionality - "quick + * turn" overrides constant-curvature turning for turn-in-place maneuvers. * - * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in - * slower output changes. Values between 1.0 and 2.0 result in - * output oscillation. Values below 0.0 and above 2.0 are - * unstable. + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. + * Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis + * [-1.0..1.0]. Clockwise is positive. + * @param allowTurnInPlace If set, overrides constant-curvature turning for + * turn-in-place maneuvers. */ - void SetQuickStopAlpha(double alpha); + static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation, + bool allowTurnInPlace); /** - * Gets if the power sent to the right side of the drivetrain is multiplied by - * -1. + * Tank drive inverse kinematics for differential drive platform. * - * @return true if the right side is inverted + * @param leftSpeed The robot left side's speed along the X axis + * [-1.0..1.0]. Forward is positive. + * @param rightSpeed The robot right side's speed along the X axis + * [-1.0..1.0]. Forward is positive. + * @param squareInputs If set, decreases the input sensitivity at low speeds. */ - bool IsRightSideInverted() const; - - /** - * Sets if the power sent to the right side of the drivetrain should be - * multiplied by -1. - * - * @param rightSideInverted true if right side power should be multiplied by - * -1 - */ - void SetRightSideInverted(bool rightSideInverted); + static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed, + bool squareInputs = true); void StopMotor() override; void GetDescription(wpi::raw_ostream& desc) const override; @@ -223,11 +213,6 @@ class DifferentialDrive : public RobotDriveBase, private: SpeedController* m_leftMotor; SpeedController* m_rightMotor; - - double m_quickStopThreshold = kDefaultQuickStopThreshold; - double m_quickStopAlpha = kDefaultQuickStopAlpha; - double m_quickStopAccumulator = 0.0; - double m_rightSideInvertMultiplier = -1.0; }; #if defined(_MSC_VER) diff --git a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h index 25f370a77a..3a3badcb6c 100644 --- a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h @@ -62,6 +62,12 @@ class KilloughDrive : public RobotDriveBase, static constexpr double kDefaultRightMotorAngle = 120.0; static constexpr double kDefaultBackMotorAngle = 270.0; + struct WheelSpeeds { + double left = 0.0; + double right = 0.0; + double back = 0.0; + }; + /** * Construct a Killough drive with the given motors and default motor angles. * @@ -134,6 +140,24 @@ class KilloughDrive : public RobotDriveBase, */ void DrivePolar(double magnitude, double angle, double zRotation); + /** + * Cartesian inverse kinematics for Killough platform. + * + * Angles are measured clockwise from the positive X axis. The robot's speed + * is independent from its angle or rotation rate. + * + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is + * positive. + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is + * positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. + * Clockwise is positive. + * @param gyroAngle The current angle reading from the gyro in degrees around + * the Z axis. Use this to implement field-oriented controls. + */ + WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation, + double gyroAngle = 0.0); + void StopMotor() override; void GetDescription(wpi::raw_ostream& desc) const override; diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h index 7c7c161f01..600de056aa 100644 --- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h @@ -76,6 +76,13 @@ class MecanumDrive : public RobotDriveBase, public Sendable, public SendableHelper { public: + struct WheelSpeeds { + double frontLeft = 0.0; + double frontRight = 0.0; + double rearLeft = 0.0; + double rearRight = 0.0; + }; + /** * Construct a MecanumDrive. * @@ -124,21 +131,22 @@ class MecanumDrive : public RobotDriveBase, void DrivePolar(double magnitude, double angle, double zRotation); /** - * Gets if the power sent to the right side of the drivetrain is multiplied by - * -1. + * Cartesian inverse kinematics for Mecanum platform. * - * @return true if the right side is inverted - */ - bool IsRightSideInverted() const; - - /** - * Sets if the power sent to the right side of the drivetrain should be - * multiplied by -1. + * Angles are measured clockwise from the positive X axis. The robot's speed + * is independent from its angle or rotation rate. * - * @param rightSideInverted true if right side power should be multiplied by - * -1 + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is + * positive. + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is + * positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. + * Clockwise is positive. + * @param gyroAngle The current angle reading from the gyro in degrees around + * the Z axis. Use this to implement field-oriented controls. */ - void SetRightSideInverted(bool rightSideInverted); + static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, + double zRotation, double gyroAngle = 0.0); void StopMotor() override; void GetDescription(wpi::raw_ostream& desc) const override; @@ -151,8 +159,6 @@ class MecanumDrive : public RobotDriveBase, SpeedController* m_frontRightMotor; SpeedController* m_rearRightMotor; - double m_rightSideInvertMultiplier = -1.0; - bool reported = false; }; diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h index 60c0ac38ac..00980d16e9 100644 --- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h +++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h @@ -76,13 +76,13 @@ class RobotDriveBase : public MotorSafety { * @param value value to clip * @param deadband range around zero */ - double ApplyDeadband(double number, double deadband); + static double ApplyDeadband(double number, double deadband); /** * Normalize all wheel speeds if the magnitude of any wheel is greater than * 1.0. */ - void Normalize(wpi::MutableArrayRef wheelSpeeds); + static void Normalize(wpi::MutableArrayRef wheelSpeeds); double m_deadband = 0.02; double m_maxOutput = 1.0; diff --git a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp new file mode 100644 index 0000000000..1d13a77f8f --- /dev/null +++ b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp @@ -0,0 +1,229 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "MockMotorController.h" +#include "frc/drive/DifferentialDrive.h" +#include "gtest/gtest.h" + +TEST(DifferentialDriveTest, ArcadeDrive) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::DifferentialDrive drive{left, right}; + drive.SetDeadband(0.0); + + // Forward + drive.ArcadeDrive(1.0, 0.0, false); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward left turn + drive.ArcadeDrive(0.5, -0.5, false); + EXPECT_DOUBLE_EQ(0.0, left.Get()); + EXPECT_DOUBLE_EQ(0.5, right.Get()); + + // Forward right turn + drive.ArcadeDrive(0.5, 0.5, false); + EXPECT_DOUBLE_EQ(0.5, left.Get()); + EXPECT_DOUBLE_EQ(0.0, right.Get()); + + // Backward + drive.ArcadeDrive(-1.0, 0.0, false); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward left turn + drive.ArcadeDrive(-0.5, -0.5, false); + EXPECT_DOUBLE_EQ(-0.5, left.Get()); + EXPECT_DOUBLE_EQ(0.0, right.Get()); + + // Backward right turn + drive.ArcadeDrive(-0.5, 0.5, false); + EXPECT_DOUBLE_EQ(0.0, left.Get()); + EXPECT_DOUBLE_EQ(-0.5, right.Get()); +} + +TEST(DifferentialDriveTest, ArcadeDriveSquared) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::DifferentialDrive drive{left, right}; + drive.SetDeadband(0.0); + + // Forward + drive.ArcadeDrive(1.0, 0.0, true); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward left turn + drive.ArcadeDrive(0.5, -0.5, true); + EXPECT_DOUBLE_EQ(0.0, left.Get()); + EXPECT_DOUBLE_EQ(0.25, right.Get()); + + // Forward right turn + drive.ArcadeDrive(0.5, 0.5, true); + EXPECT_DOUBLE_EQ(0.25, left.Get()); + EXPECT_DOUBLE_EQ(0.0, right.Get()); + + // Backward + drive.ArcadeDrive(-1.0, 0.0, true); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward left turn + drive.ArcadeDrive(-0.5, -0.5, true); + EXPECT_DOUBLE_EQ(-0.25, left.Get()); + EXPECT_DOUBLE_EQ(0.0, right.Get()); + + // Backward right turn + drive.ArcadeDrive(-0.5, 0.5, true); + EXPECT_DOUBLE_EQ(0.0, left.Get()); + EXPECT_DOUBLE_EQ(-0.25, right.Get()); +} + +TEST(DifferentialDriveTest, CurvatureDrive) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::DifferentialDrive drive{left, right}; + drive.SetDeadband(0.0); + + // Forward + drive.CurvatureDrive(1.0, 0.0, false); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward left turn + drive.CurvatureDrive(0.5, -0.5, false); + EXPECT_DOUBLE_EQ(0.25, left.Get()); + EXPECT_DOUBLE_EQ(0.75, right.Get()); + + // Forward right turn + drive.CurvatureDrive(0.5, 0.5, false); + EXPECT_DOUBLE_EQ(0.75, left.Get()); + EXPECT_DOUBLE_EQ(0.25, right.Get()); + + // Backward + drive.CurvatureDrive(-1.0, 0.0, false); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward left turn + drive.CurvatureDrive(-0.5, -0.5, false); + EXPECT_DOUBLE_EQ(-0.75, left.Get()); + EXPECT_DOUBLE_EQ(-0.25, right.Get()); + + // Backward right turn + drive.CurvatureDrive(-0.5, 0.5, false); + EXPECT_DOUBLE_EQ(-0.25, left.Get()); + EXPECT_DOUBLE_EQ(-0.75, right.Get()); +} + +TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::DifferentialDrive drive{left, right}; + drive.SetDeadband(0.0); + + // Forward + drive.CurvatureDrive(1.0, 0.0, true); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward left turn + drive.CurvatureDrive(0.5, -0.5, true); + EXPECT_DOUBLE_EQ(0.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward right turn + drive.CurvatureDrive(0.5, 0.5, true); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(0.0, right.Get()); + + // Backward + drive.CurvatureDrive(-1.0, 0.0, true); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward left turn + drive.CurvatureDrive(-0.5, -0.5, true); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(0.0, right.Get()); + + // Backward right turn + drive.CurvatureDrive(-0.5, 0.5, true); + EXPECT_DOUBLE_EQ(0.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); +} + +TEST(DifferentialDriveTest, TankDrive) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::DifferentialDrive drive{left, right}; + drive.SetDeadband(0.0); + + // Forward + drive.TankDrive(1.0, 1.0, false); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward left turn + drive.TankDrive(0.5, 1.0, false); + EXPECT_DOUBLE_EQ(0.5, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward right turn + drive.TankDrive(1.0, 0.5, false); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(0.5, right.Get()); + + // Backward + drive.TankDrive(-1.0, -1.0, false); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward left turn + drive.TankDrive(-0.5, -1.0, false); + EXPECT_DOUBLE_EQ(-0.5, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward right turn + drive.TankDrive(-0.5, 1.0, false); + EXPECT_DOUBLE_EQ(-0.5, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); +} + +TEST(DifferentialDriveTest, TankDriveSquared) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::DifferentialDrive drive{left, right}; + drive.SetDeadband(0.0); + + // Forward + drive.TankDrive(1.0, 1.0, true); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward left turn + drive.TankDrive(0.5, 1.0, true); + EXPECT_DOUBLE_EQ(0.25, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + + // Forward right turn + drive.TankDrive(1.0, 0.5, true); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(0.25, right.Get()); + + // Backward + drive.TankDrive(-1.0, -1.0, true); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward left turn + drive.TankDrive(-0.5, -1.0, true); + EXPECT_DOUBLE_EQ(-0.25, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + + // Backward right turn + drive.TankDrive(-1.0, -0.5, true); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-0.25, right.Get()); +} diff --git a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp new file mode 100644 index 0000000000..510b377c55 --- /dev/null +++ b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp @@ -0,0 +1,123 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "MockMotorController.h" +#include "frc/drive/KilloughDrive.h" +#include "gtest/gtest.h" + +TEST(KilloughDriveTest, Cartesian) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::MockMotorController back; + frc::KilloughDrive drive{left, right, back}; + drive.SetDeadband(0.0); + + // Forward + drive.DriveCartesian(1.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(0.5, left.Get()); + EXPECT_DOUBLE_EQ(-0.5, right.Get()); + EXPECT_NEAR(0.0, back.Get(), 1e-9); + + // Left + drive.DriveCartesian(0.0, -1.0, 0.0); + EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, left.Get()); + EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, right.Get()); + EXPECT_DOUBLE_EQ(1.0, back.Get()); + + // Right + drive.DriveCartesian(0.0, 1.0, 0.0); + EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, left.Get()); + EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, right.Get()); + EXPECT_DOUBLE_EQ(-1.0, back.Get()); + + // Rotate CCW + drive.DriveCartesian(0.0, 0.0, -1.0); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + EXPECT_DOUBLE_EQ(-1.0, back.Get()); + + // Rotate CW + drive.DriveCartesian(0.0, 0.0, 1.0); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + EXPECT_DOUBLE_EQ(1.0, back.Get()); +} + +TEST(KilloughDriveTest, CartesianGyro90CW) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::MockMotorController back; + frc::KilloughDrive drive{left, right, back}; + drive.SetDeadband(0.0); + + // Forward in global frame; left in robot frame + drive.DriveCartesian(1.0, 0.0, 0.0, 90.0); + EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, left.Get()); + EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, right.Get()); + EXPECT_DOUBLE_EQ(1.0, back.Get()); + + // Left in global frame; backward in robot frame + drive.DriveCartesian(0.0, -1.0, 0.0, 90.0); + EXPECT_DOUBLE_EQ(-0.5, left.Get()); + EXPECT_NEAR(0.5, right.Get(), 1e-9); + EXPECT_NEAR(0.0, back.Get(), 1e-9); + + // Right in global frame; forward in robot frame + drive.DriveCartesian(0.0, 1.0, 0.0, 90.0); + EXPECT_DOUBLE_EQ(0.5, left.Get()); + EXPECT_NEAR(-0.5, right.Get(), 1e-9); + EXPECT_NEAR(0.0, back.Get(), 1e-9); + + // Rotate CCW + drive.DriveCartesian(0.0, 0.0, -1.0, 90.0); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + EXPECT_DOUBLE_EQ(-1.0, back.Get()); + + // Rotate CW + drive.DriveCartesian(0.0, 0.0, 1.0, 90.0); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + EXPECT_DOUBLE_EQ(1.0, back.Get()); +} + +TEST(KilloughDriveTest, Polar) { + frc::MockMotorController left; + frc::MockMotorController right; + frc::MockMotorController back; + frc::KilloughDrive drive{left, right, back}; + drive.SetDeadband(0.0); + + // Forward + drive.DrivePolar(1.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, left.Get()); + EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, right.Get()); + EXPECT_DOUBLE_EQ(-1.0, back.Get()); + + // Left + drive.DrivePolar(1.0, -90.0, 0.0); + EXPECT_DOUBLE_EQ(-0.5, left.Get()); + EXPECT_DOUBLE_EQ(0.5, right.Get()); + EXPECT_NEAR(0.0, back.Get(), 1e-9); + + // Right + drive.DrivePolar(1.0, 90.0, 0.0); + EXPECT_DOUBLE_EQ(0.5, left.Get()); + EXPECT_NEAR(-0.5, right.Get(), 1e-9); + EXPECT_NEAR(0.0, back.Get(), 1e-9); + + // Rotate CCW + drive.DrivePolar(0.0, 0.0, -1.0); + EXPECT_DOUBLE_EQ(-1.0, left.Get()); + EXPECT_DOUBLE_EQ(-1.0, right.Get()); + EXPECT_DOUBLE_EQ(-1.0, back.Get()); + + // Rotate CW + drive.DrivePolar(0.0, 0.0, 1.0); + EXPECT_DOUBLE_EQ(1.0, left.Get()); + EXPECT_DOUBLE_EQ(1.0, right.Get()); + EXPECT_DOUBLE_EQ(1.0, back.Get()); +} diff --git a/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp new file mode 100644 index 0000000000..fe0a73bd4e --- /dev/null +++ b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp @@ -0,0 +1,139 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "MockMotorController.h" +#include "frc/drive/MecanumDrive.h" +#include "gtest/gtest.h" + +TEST(MecanumDriveTest, Cartesian) { + frc::MockMotorController fl; + frc::MockMotorController fr; + frc::MockMotorController rl; + frc::MockMotorController rr; + frc::MecanumDrive drive{fl, fr, rl, rr}; + drive.SetDeadband(0.0); + + // Forward + drive.DriveCartesian(1.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Left + drive.DriveCartesian(0.0, -1.0, 0.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); + + // Right + drive.DriveCartesian(0.0, 1.0, 0.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(-1.0, fr.Get()); + EXPECT_DOUBLE_EQ(-1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Rotate CCW + drive.DriveCartesian(0.0, 0.0, -1.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(-1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Rotate CW + drive.DriveCartesian(0.0, 0.0, 1.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(-1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); +} + +TEST(MecanumDriveTest, CartesianGyro90CW) { + frc::MockMotorController fl; + frc::MockMotorController fr; + frc::MockMotorController rl; + frc::MockMotorController rr; + frc::MecanumDrive drive{fl, fr, rl, rr}; + drive.SetDeadband(0.0); + + // Forward in global frame; left in robot frame + drive.DriveCartesian(1.0, 0.0, 0.0, 90.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); + + // Left in global frame; backward in robot frame + drive.DriveCartesian(0.0, -1.0, 0.0, 90.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(-1.0, fr.Get()); + EXPECT_DOUBLE_EQ(-1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); + + // Right in global frame; forward in robot frame + drive.DriveCartesian(0.0, 1.0, 0.0, 90.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Rotate CCW + drive.DriveCartesian(0.0, 0.0, -1.0, 90.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(-1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Rotate CW + drive.DriveCartesian(0.0, 0.0, 1.0, 90.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(-1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); +} + +TEST(MecanumDriveTest, Polar) { + frc::MockMotorController fl; + frc::MockMotorController fr; + frc::MockMotorController rl; + frc::MockMotorController rr; + frc::MecanumDrive drive{fl, fr, rl, rr}; + drive.SetDeadband(0.0); + + // Forward + drive.DrivePolar(1.0, 0.0, 0.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Left + drive.DrivePolar(1.0, -90.0, 0.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); + + // Right + drive.DrivePolar(1.0, 90.0, 0.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(-1.0, fr.Get()); + EXPECT_DOUBLE_EQ(-1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Rotate CCW + drive.DrivePolar(0.0, 0.0, -1.0); + EXPECT_DOUBLE_EQ(-1.0, fl.Get()); + EXPECT_DOUBLE_EQ(-1.0, fr.Get()); + EXPECT_DOUBLE_EQ(1.0, rl.Get()); + EXPECT_DOUBLE_EQ(1.0, rr.Get()); + + // Rotate CW + drive.DrivePolar(0.0, 0.0, 1.0); + EXPECT_DOUBLE_EQ(1.0, fl.Get()); + EXPECT_DOUBLE_EQ(1.0, fr.Get()); + EXPECT_DOUBLE_EQ(-1.0, rl.Get()); + EXPECT_DOUBLE_EQ(-1.0, rr.Get()); +} 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 90188ed91e..5944854408 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 @@ -83,32 +83,36 @@ import java.util.StringJoiner; *

Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will * be set to 0, and larger values will be scaled so that the full range is still used. This deadband * value can be changed with {@link #setDeadband}. - * - *

RobotDrive porting guide:
- * {@link #tankDrive(double, double)} is equivalent to RobotDrive's tankDrive(double, double) if a - * deadband of 0 is used.
- * {@link #arcadeDrive(double, double)} is equivalent to RobotDrive's arcadeDrive(double, double) if - * a deadband of 0 is used and the the rotation input is inverted eg arcadeDrive(y, -rotation)
- * {@link #curvatureDrive(double, double, boolean)} is similar in concept to RobotDrive's - * drive(double, double) with the addition of a quick turn mode. However, it is not designed to give - * exactly the same response. */ @SuppressWarnings("removal") public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable { - public static final double kDefaultQuickStopThreshold = 0.2; - public static final double kDefaultQuickStopAlpha = 0.1; - private static int instances; private final SpeedController m_leftMotor; private final SpeedController m_rightMotor; - private double m_quickStopThreshold = kDefaultQuickStopThreshold; - private double m_quickStopAlpha = kDefaultQuickStopAlpha; - private double m_quickStopAccumulator; - private double m_rightSideInvertMultiplier = -1.0; private boolean m_reported; + @SuppressWarnings("MemberName") + public static class WheelSpeeds { + public double left; + public double right; + + /** Constructs a WheelSpeeds with zeroes for left and right speeds. */ + public WheelSpeeds() {} + + /** + * Constructs a WheelSpeeds. + * + * @param left The left speed. + * @param right The right speed. + */ + public WheelSpeeds(double left, double right) { + this.left = left; + this.right = right; + } + } + /** * Construct a DifferentialDrive. * @@ -183,47 +187,13 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC m_reported = true; } - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); xSpeed = applyDeadband(xSpeed, m_deadband); - - zRotation = MathUtil.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 = Math.copySign(xSpeed * xSpeed, xSpeed); - zRotation = Math.copySign(zRotation * zRotation, zRotation); - } + var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs); - double leftMotorOutput; - double rightMotorOutput; - - double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.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(MathUtil.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput); - double maxOutput = m_maxOutput * m_rightSideInvertMultiplier; - m_rightMotor.set(MathUtil.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput); + m_leftMotor.set(speeds.left * m_maxOutput); + m_rightMotor.set(speeds.right * m_maxOutput); feed(); } @@ -239,75 +209,24 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is * positive. - * @param isQuickTurn If set, overrides constant-curvature turning for turn-in-place maneuvers. + * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place + * maneuvers. */ @SuppressWarnings({"ParameterName", "PMD.CyclomaticComplexity"}) - public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) { + public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) { if (!m_reported) { HAL.report( tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialCurvature, 2); m_reported = true; } - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); xSpeed = applyDeadband(xSpeed, m_deadband); - - zRotation = MathUtil.clamp(zRotation, -1.0, 1.0); zRotation = applyDeadband(zRotation, m_deadband); - double angularPower; - boolean overPower; + var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace); - if (isQuickTurn) { - if (Math.abs(xSpeed) < m_quickStopThreshold) { - m_quickStopAccumulator = - (1 - m_quickStopAlpha) * m_quickStopAccumulator - + m_quickStopAlpha * MathUtil.clamp(zRotation, -1.0, 1.0) * 2; - } - overPower = true; - angularPower = zRotation; - } else { - overPower = false; - angularPower = Math.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 = 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 * m_rightSideInvertMultiplier); + m_leftMotor.set(speeds.left * m_maxOutput); + m_rightMotor.set(speeds.right * m_maxOutput); feed(); } @@ -339,12 +258,125 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC m_reported = true; } - leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0); leftSpeed = applyDeadband(leftSpeed, m_deadband); - - rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0); rightSpeed = applyDeadband(rightSpeed, m_deadband); + var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs); + + m_leftMotor.set(speeds.left * m_maxOutput); + m_rightMotor.set(speeds.right * m_maxOutput); + + feed(); + } + + /** + * Arcade drive inverse kinematics for differential drive platform. + * + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + * @param squareInputs If set, decreases the input sensitivity at low speeds. + */ + @SuppressWarnings("ParameterName") + public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) { + xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); + zRotation = MathUtil.clamp(zRotation, -1.0, 1.0); + + // Square the inputs (while preserving the sign) to increase fine control + // while permitting full power. + if (squareInputs) { + xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); + zRotation = Math.copySign(zRotation * zRotation, zRotation); + } + + double leftSpeed; + double rightSpeed; + + double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.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 = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); + if (maxMagnitude > 1.0) { + leftSpeed /= maxMagnitude; + rightSpeed /= maxMagnitude; + } + + return new WheelSpeeds(leftSpeed, rightSpeed); + } + + /** + * Curvature drive inverse kinematics for differential drive platform. + * + *

The rotation argument controls the curvature of the robot's path rather than its rate of + * heading change. This makes the robot more controllable at high speeds. Also handles the robot's + * quick turn functionality - "quick turn" overrides constant-curvature turning for turn-in-place + * maneuvers. + * + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place + * maneuvers. + */ + @SuppressWarnings("ParameterName") + public static WheelSpeeds curvatureDriveIK( + double xSpeed, double zRotation, boolean allowTurnInPlace) { + xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); + zRotation = MathUtil.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 + Math.abs(xSpeed) * zRotation; + rightSpeed = xSpeed - Math.abs(xSpeed) * zRotation; + } + + // Normalize wheel speeds + double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); + if (maxMagnitude > 1.0) { + leftSpeed /= maxMagnitude; + rightSpeed /= maxMagnitude; + } + + return new WheelSpeeds(leftSpeed, rightSpeed); + } + + /** + * Tank drive inverse kinematics for differential drive platform. + * + * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is + * positive. + * @param squareInputs If set, decreases the input sensitivity at low speeds. + */ + public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) { + leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0); + rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0); + // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { @@ -352,59 +384,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed); } - m_leftMotor.set(leftSpeed * m_maxOutput); - m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier); - - feed(); - } - - /** - * Sets the QuickStop speed threshold in curvature drive. - * - *

QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn. - * - *

While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value - * outputted by the low-pass filter when the robot's speed along the X axis is below the - * threshold. When QuickTurn is disabled, the accumulator's value is applied against the computed - * angular power request to slow the robot's rotation. - * - * @param threshold X speed below which quick stop accumulator will receive rotation rate values - * [0..1.0]. - */ - public void setQuickStopThreshold(double threshold) { - m_quickStopThreshold = threshold; - } - - /** - * Sets the low-pass filter gain for QuickStop in curvature drive. - * - *

The low-pass filter filters incoming rotation rate commands to smooth out high frequency - * changes. - * - * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes. - * Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and above 2.0 are - * unstable. - */ - public void setQuickStopAlpha(double alpha) { - m_quickStopAlpha = alpha; - } - - /** - * Gets if the power sent to the right side of the drivetrain is multiplied 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 multiplied by -1. - * - * @param rightSideInverted true if right side power should be multiplied by -1 - */ - public void setRightSideInverted(boolean rightSideInverted) { - m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0; + return new WheelSpeeds(leftSpeed, rightSpeed); } @Override @@ -426,8 +406,6 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC builder.setSafeState(this::stopMotor); builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set); builder.addDoubleProperty( - "Right Motor Speed", - () -> m_rightMotor.get() * m_rightSideInvertMultiplier, - x -> m_rightMotor.set(x * m_rightSideInvertMultiplier)); + "Right Motor Speed", () -> m_rightMotor.get(), x -> m_rightMotor.set(x)); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java index b7dff3db4c..c9c5ac5204 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java @@ -57,6 +57,29 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose private boolean m_reported; + @SuppressWarnings("MemberName") + public static class WheelSpeeds { + public double left; + public double right; + public double back; + + /** Constructs a WheelSpeeds with zeroes for left, right, and back speeds. */ + public WheelSpeeds() {} + + /** + * Constructs a WheelSpeeds. + * + * @param left The left speed. + * @param right The right speed. + * @param back The back speed. + */ + public WheelSpeeds(double left, double right, double back) { + this.left = left; + this.right = right; + this.back = back; + } + } + /** * Construct a Killough drive with the given motors and default motor angles. * @@ -191,26 +214,14 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose m_reported = true; } - ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); ySpeed = applyDeadband(ySpeed, m_deadband); - - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); xSpeed = applyDeadband(xSpeed, m_deadband); - // Compensate for gyro angle. - Vector2d input = new Vector2d(ySpeed, xSpeed); - input.rotate(-gyroAngle); + var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); - double[] wheelSpeeds = new double[3]; - wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation; - wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation; - wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation; - - normalize(wheelSpeeds); - - m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput); - m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput); - m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * m_maxOutput); + m_leftMotor.set(speeds.left * m_maxOutput); + m_rightMotor.set(speeds.right * m_maxOutput); + m_backMotor.set(speeds.back * m_maxOutput); feed(); } @@ -240,6 +251,42 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose 0.0); } + /** + * Cartesian inverse kinematics for Killough platform. + * + *

Angles are measured clockwise from the positive X axis. The robot's speed is independent + * from its angle or rotation rate. + * + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive. + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this + * to implement field-oriented controls. + */ + @SuppressWarnings("ParameterName") + public WheelSpeeds driveCartesianIK( + double ySpeed, double xSpeed, double zRotation, double gyroAngle) { + ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); + xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); + + // Compensate for gyro angle. + Vector2d input = new Vector2d(ySpeed, xSpeed); + input.rotate(-gyroAngle); + + double[] wheelSpeeds = new double[3]; + wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation; + wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation; + wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation; + + normalize(wheelSpeeds); + + return new WheelSpeeds( + wheelSpeeds[MotorType.kLeft.value], + wheelSpeeds[MotorType.kRight.value], + wheelSpeeds[MotorType.kBack.value]); + } + @Override public void stopMotor() { m_leftMotor.stopMotor(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index 0fda4a0bfa..4aa92b11b6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -66,9 +66,34 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea private final SpeedController m_frontRightMotor; private final SpeedController m_rearRightMotor; - private double m_rightSideInvertMultiplier = -1.0; private boolean m_reported; + @SuppressWarnings("MemberName") + public static class WheelSpeeds { + public double frontLeft; + public double frontRight; + public double rearLeft; + public double rearRight; + + /** Constructs a WheelSpeeds with zeroes for all four speeds. */ + public WheelSpeeds() {} + + /** + * Constructs a WheelSpeeds. + * + * @param frontLeft The front left speed. + * @param frontRight The front right speed. + * @param rearLeft The rear left speed. + * @param rearRight The rear right speed. + */ + public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) { + this.frontLeft = frontLeft; + this.frontRight = frontRight; + this.rearLeft = rearLeft; + this.rearRight = rearRight; + } + } + /** * Construct a MecanumDrive. * @@ -167,30 +192,15 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea m_reported = true; } - ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); ySpeed = applyDeadband(ySpeed, m_deadband); - - xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); xSpeed = applyDeadband(xSpeed, m_deadband); - // Compensate for gyro angle. - Vector2d input = new Vector2d(ySpeed, xSpeed); - input.rotate(-gyroAngle); + var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); - 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.kRearLeft.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_rightSideInvertMultiplier); - m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput); - m_rearRightMotor.set( - wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput * m_rightSideInvertMultiplier); + m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput); + m_frontRightMotor.set(speeds.frontRight * m_maxOutput); + m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput); + m_rearRightMotor.set(speeds.rearRight * m_maxOutput); feed(); } @@ -214,28 +224,48 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea } driveCartesian( - magnitude * Math.sin(angle * (Math.PI / 180.0)), magnitude * Math.cos(angle * (Math.PI / 180.0)), + magnitude * Math.sin(angle * (Math.PI / 180.0)), zRotation, 0.0); } /** - * Gets if the power sent to the right side of the drivetrain is multiplied by -1. + * Cartesian inverse kinematics for Mecanum platform. * - * @return true if the right side is inverted + *

Angles are measured clockwise from the positive X axis. The robot's speed is independent + * from its angle or rotation rate. + * + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive. + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * positive. + * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this + * to implement field-oriented controls. */ - public boolean isRightSideInverted() { - return m_rightSideInvertMultiplier == -1.0; - } + @SuppressWarnings("ParameterName") + public static WheelSpeeds driveCartesianIK( + double ySpeed, double xSpeed, double zRotation, double gyroAngle) { + ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); + xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); - /** - * Sets if the power sent to the right side of the drivetrain should be multiplied by -1. - * - * @param rightSideInverted true if right side power should be multiplied by -1 - */ - public void setRightSideInverted(boolean rightSideInverted) { - m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0; + // Compensate for gyro angle. + Vector2d input = new Vector2d(ySpeed, xSpeed); + input.rotate(-gyroAngle); + + 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.kRearLeft.value] = input.x - input.y + zRotation; + wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation; + + normalize(wheelSpeeds); + + return new WheelSpeeds( + wheelSpeeds[MotorType.kFrontLeft.value], + wheelSpeeds[MotorType.kFrontRight.value], + wheelSpeeds[MotorType.kRearLeft.value], + wheelSpeeds[MotorType.kRearRight.value]); } @Override @@ -261,12 +291,12 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set); builder.addDoubleProperty( "Front Right Motor Speed", - () -> m_frontRightMotor.get() * m_rightSideInvertMultiplier, - value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier)); + () -> m_frontRightMotor.get(), + value -> m_frontRightMotor.set(value)); builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set); builder.addDoubleProperty( "Rear Right Motor Speed", - () -> m_rearRightMotor.get() * m_rightSideInvertMultiplier, - value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier)); + () -> m_rearRightMotor.get(), + value -> m_rearRightMotor.set(value)); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java index fae7df75c9..b954143dcf 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java @@ -83,7 +83,7 @@ public abstract class RobotDriveBase extends MotorSafety { * @param value value to clip * @param deadband range around zero */ - protected double applyDeadband(double value, double deadband) { + protected static double applyDeadband(double value, double deadband) { if (Math.abs(value) > deadband) { if (value > 0.0) { return (value - deadband) / (1.0 - deadband); @@ -96,7 +96,7 @@ public abstract class RobotDriveBase extends MotorSafety { } /** Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */ - protected void normalize(double[] wheelSpeeds) { + protected static void normalize(double[] wheelSpeeds) { double maxMagnitude = Math.abs(wheelSpeeds[0]); for (int i = 1; i < wheelSpeeds.length; i++) { double temp = Math.abs(wheelSpeeds[i]); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java new file mode 100644 index 0000000000..e7f2e3178b --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java @@ -0,0 +1,240 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.wpilibj.motorcontrol.MockMotorController; +import org.junit.jupiter.api.Test; + +class DifferentialDriveTest { + @Test + void testArcadeDrive() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var drive = new DifferentialDrive(left, right); + drive.setDeadband(0.0); + + // Forward + drive.arcadeDrive(1.0, 0.0, false); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward left turn + drive.arcadeDrive(0.5, -0.5, false); + assertEquals(0.0, left.get(), 1e-9); + assertEquals(0.5, right.get(), 1e-9); + + // Forward right turn + drive.arcadeDrive(0.5, 0.5, false); + assertEquals(0.5, left.get(), 1e-9); + assertEquals(0.0, right.get(), 1e-9); + + // Backward + drive.arcadeDrive(-1.0, 0.0, false); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward left turn + drive.arcadeDrive(-0.5, -0.5, false); + assertEquals(-0.5, left.get(), 1e-9); + assertEquals(0.0, right.get(), 1e-9); + + // Backward right turn + drive.arcadeDrive(-0.5, 0.5, false); + assertEquals(0.0, left.get(), 1e-9); + assertEquals(-0.5, right.get(), 1e-9); + } + + @Test + void testArcadeDriveSquared() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var drive = new DifferentialDrive(left, right); + drive.setDeadband(0.0); + + // Forward + drive.arcadeDrive(1.0, 0.0, true); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward left turn + drive.arcadeDrive(0.5, -0.5, true); + assertEquals(0.0, left.get(), 1e-9); + assertEquals(0.25, right.get(), 1e-9); + + // Forward right turn + drive.arcadeDrive(0.5, 0.5, true); + assertEquals(0.25, left.get(), 1e-9); + assertEquals(0.0, right.get(), 1e-9); + + // Backward + drive.arcadeDrive(-1.0, 0.0, true); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward left turn + drive.arcadeDrive(-0.5, -0.5, true); + assertEquals(-0.25, left.get(), 1e-9); + assertEquals(0.0, right.get(), 1e-9); + + // Backward right turn + drive.arcadeDrive(-0.5, 0.5, true); + assertEquals(0.0, left.get(), 1e-9); + assertEquals(-0.25, right.get(), 1e-9); + } + + @Test + void testCurvatureDrive() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var drive = new DifferentialDrive(left, right); + drive.setDeadband(0.0); + + // Forward + drive.curvatureDrive(1.0, 0.0, false); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward left turn + drive.curvatureDrive(0.5, -0.5, false); + assertEquals(0.25, left.get(), 1e-9); + assertEquals(0.75, right.get(), 1e-9); + + // Forward right turn + drive.curvatureDrive(0.5, 0.5, false); + assertEquals(0.75, left.get(), 1e-9); + assertEquals(0.25, right.get(), 1e-9); + + // Backward + drive.curvatureDrive(-1.0, 0.0, false); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward left turn + drive.curvatureDrive(-0.5, -0.5, false); + assertEquals(-0.75, left.get(), 1e-9); + assertEquals(-0.25, right.get(), 1e-9); + + // Backward right turn + drive.curvatureDrive(-0.5, 0.5, false); + assertEquals(-0.25, left.get(), 1e-9); + assertEquals(-0.75, right.get(), 1e-9); + } + + @Test + void testCurvatureDriveTurnInPlace() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var drive = new DifferentialDrive(left, right); + drive.setDeadband(0.0); + + // Forward + drive.curvatureDrive(1.0, 0.0, true); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward left turn + drive.curvatureDrive(0.5, -0.5, true); + assertEquals(0.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward right turn + drive.curvatureDrive(0.5, 0.5, true); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(0.0, right.get(), 1e-9); + + // Backward + drive.curvatureDrive(-1.0, 0.0, true); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward left turn + drive.curvatureDrive(-0.5, -0.5, true); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(0.0, right.get(), 1e-9); + + // Backward right turn + drive.curvatureDrive(-0.5, 0.5, true); + assertEquals(0.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + } + + @Test + void testTankDrive() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var drive = new DifferentialDrive(left, right); + drive.setDeadband(0.0); + + // Forward + drive.tankDrive(1.0, 1.0, false); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward left turn + drive.tankDrive(0.5, 1.0, false); + assertEquals(0.5, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward right turn + drive.tankDrive(1.0, 0.5, false); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(0.5, right.get(), 1e-9); + + // Backward + drive.tankDrive(-1.0, -1.0, false); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward left turn + drive.tankDrive(-0.5, -1.0, false); + assertEquals(-0.5, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward right turn + drive.tankDrive(-0.5, 1.0, false); + assertEquals(-0.5, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + } + + @Test + void testTankDriveSquared() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var drive = new DifferentialDrive(left, right); + drive.setDeadband(0.0); + + // Forward + drive.tankDrive(1.0, 1.0, true); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward left turn + drive.tankDrive(0.5, 1.0, true); + assertEquals(0.25, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + + // Forward right turn + drive.tankDrive(1.0, 0.5, true); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(0.25, right.get(), 1e-9); + + // Backward + drive.tankDrive(-1.0, -1.0, true); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward left turn + drive.tankDrive(-0.5, -1.0, true); + assertEquals(-0.25, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + + // Backward right turn + drive.tankDrive(-1.0, -0.5, true); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-0.25, right.get(), 1e-9); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java new file mode 100644 index 0000000000..7d06d9f818 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.wpilibj.motorcontrol.MockMotorController; +import org.junit.jupiter.api.Test; + +class KilloughDriveTest { + @Test + void testCartesian() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var back = new MockMotorController(); + var drive = new KilloughDrive(left, right, back); + drive.setDeadband(0.0); + + // Forward + drive.driveCartesian(1.0, 0.0, 0.0); + assertEquals(0.5, left.get(), 1e-9); + assertEquals(-0.5, right.get(), 1e-9); + assertEquals(0.0, back.get(), 1e-9); + + // Left + drive.driveCartesian(0.0, -1.0, 0.0); + assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9); + assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9); + assertEquals(1.0, back.get(), 1e-9); + + // Right + drive.driveCartesian(0.0, 1.0, 0.0); + assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9); + assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9); + assertEquals(-1.0, back.get(), 1e-9); + + // Rotate CCW + drive.driveCartesian(0.0, 0.0, -1.0); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + assertEquals(-1.0, back.get(), 1e-9); + + // Rotate CW + drive.driveCartesian(0.0, 0.0, 1.0); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + assertEquals(1.0, back.get(), 1e-9); + } + + @Test + void testCartesiangyro90CW() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var back = new MockMotorController(); + var drive = new KilloughDrive(left, right, back); + drive.setDeadband(0.0); + + // Forward in global frame; left in robot frame + drive.driveCartesian(1.0, 0.0, 0.0, 90.0); + assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9); + assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9); + assertEquals(1.0, back.get(), 1e-9); + + // Left in global frame; backward in robot frame + drive.driveCartesian(0.0, -1.0, 0.0, 90.0); + assertEquals(-0.5, left.get(), 1e-9); + assertEquals(0.5, right.get(), 1e-9); + assertEquals(0.0, back.get(), 1e-9); + + // Right in global frame; forward in robot frame + drive.driveCartesian(0.0, 1.0, 0.0, 90.0); + assertEquals(0.5, left.get(), 1e-9); + assertEquals(-0.5, right.get(), 1e-9); + assertEquals(0.0, back.get(), 1e-9); + + // Rotate CCW + drive.driveCartesian(0.0, 0.0, -1.0, 90.0); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + assertEquals(-1.0, back.get(), 1e-9); + + // Rotate CW + drive.driveCartesian(0.0, 0.0, 1.0, 90.0); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + assertEquals(1.0, back.get(), 1e-9); + } + + @Test + void testPolar() { + var left = new MockMotorController(); + var right = new MockMotorController(); + var back = new MockMotorController(); + var drive = new KilloughDrive(left, right, back); + drive.setDeadband(0.0); + + // Forward + drive.drivePolar(1.0, 0.0, 0.0); + assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9); + assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9); + assertEquals(-1.0, back.get(), 1e-9); + + // Left + drive.drivePolar(1.0, -90.0, 0.0); + assertEquals(-0.5, left.get(), 1e-9); + assertEquals(0.5, right.get(), 1e-9); + assertEquals(0.0, back.get(), 1e-9); + + // Right + drive.drivePolar(1.0, 90.0, 0.0); + assertEquals(0.5, left.get(), 1e-9); + assertEquals(-0.5, right.get(), 1e-9); + assertEquals(0.0, back.get(), 1e-9); + + // Rotate CCW + drive.drivePolar(0.0, 0.0, -1.0); + assertEquals(-1.0, left.get(), 1e-9); + assertEquals(-1.0, right.get(), 1e-9); + assertEquals(-1.0, back.get(), 1e-9); + + // Rotate CW + drive.drivePolar(0.0, 0.0, 1.0); + assertEquals(1.0, left.get(), 1e-9); + assertEquals(1.0, right.get(), 1e-9); + assertEquals(1.0, back.get(), 1e-9); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java new file mode 100644 index 0000000000..70a07a2226 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java @@ -0,0 +1,147 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.wpilibj.motorcontrol.MockMotorController; +import org.junit.jupiter.api.Test; + +class MecanumDriveTest { + @Test + void testCartesian() { + var fl = new MockMotorController(); + var fr = new MockMotorController(); + var rl = new MockMotorController(); + var rr = new MockMotorController(); + var drive = new MecanumDrive(fl, fr, rl, rr); + drive.setDeadband(0.0); + + // Forward + drive.driveCartesian(1.0, 0.0, 0.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Left + drive.driveCartesian(0.0, -1.0, 0.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + + // Right + drive.driveCartesian(0.0, 1.0, 0.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(-1.0, fr.get(), 1e-9); + assertEquals(-1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Rotate CCW + drive.driveCartesian(0.0, 0.0, -1.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(-1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Rotate CW + drive.driveCartesian(0.0, 0.0, 1.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(-1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + } + + @Test + void testCartesiangyro90CW() { + var fl = new MockMotorController(); + var fr = new MockMotorController(); + var rl = new MockMotorController(); + var rr = new MockMotorController(); + var drive = new MecanumDrive(fl, fr, rl, rr); + drive.setDeadband(0.0); + + // Forward in global frame; left in robot frame + drive.driveCartesian(1.0, 0.0, 0.0, 90.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + + // Left in global frame; backward in robot frame + drive.driveCartesian(0.0, -1.0, 0.0, 90.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(-1.0, fr.get(), 1e-9); + assertEquals(-1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + + // Right in global frame; forward in robot frame + drive.driveCartesian(0.0, 1.0, 0.0, 90.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Rotate CCW + drive.driveCartesian(0.0, 0.0, -1.0, 90.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(-1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Rotate CW + drive.driveCartesian(0.0, 0.0, 1.0, 90.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(-1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + } + + @Test + void testPolar() { + var fl = new MockMotorController(); + var fr = new MockMotorController(); + var rl = new MockMotorController(); + var rr = new MockMotorController(); + var drive = new MecanumDrive(fl, fr, rl, rr); + drive.setDeadband(0.0); + + // Forward + drive.drivePolar(1.0, 0.0, 0.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Left + drive.drivePolar(1.0, -90.0, 0.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + + // Right + drive.drivePolar(1.0, 90.0, 0.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(-1.0, fr.get(), 1e-9); + assertEquals(-1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Rotate CCW + drive.drivePolar(0.0, 0.0, -1.0); + assertEquals(-1.0, fl.get(), 1e-9); + assertEquals(-1.0, fr.get(), 1e-9); + assertEquals(1.0, rl.get(), 1e-9); + assertEquals(1.0, rr.get(), 1e-9); + + // Rotate CW + drive.drivePolar(0.0, 0.0, 1.0); + assertEquals(1.0, fl.get(), 1e-9); + assertEquals(1.0, fr.get(), 1e-9); + assertEquals(-1.0, rl.get(), 1e-9); + assertEquals(-1.0, rr.get(), 1e-9); + } +}