diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp index a541eb1d24..367dd6f846 100644 --- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp @@ -98,39 +98,19 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK( zRotation = std::copysign(zRotation * zRotation, zRotation); } - double leftSpeed; - double rightSpeed; + double leftSpeed = xSpeed - zRotation; + double rightSpeed = xSpeed + zRotation; - double maxInput = - std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed); - - // Sign is used because `xSpeed >= 0.0` succeeds for -0.0 - if (!std::signbit(xSpeed)) { - // First quadrant, else second quadrant - if (!std::signbit(zRotation)) { - leftSpeed = maxInput; - rightSpeed = xSpeed - zRotation; - } else { - leftSpeed = xSpeed + zRotation; - rightSpeed = maxInput; - } - } else { - // Third quadrant, else fourth quadrant - if (!std::signbit(zRotation)) { - 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; + // Find the maximum possible value of (throttle + turn) along the vector that + // the joystick is pointing, then desaturate the wheel speeds + double greaterInput = std::max(std::abs(xSpeed), std::abs(zRotation)); + double lesserInput = std::min(std::abs(xSpeed), std::abs(zRotation)); + if (greaterInput == 0.0) { + return {0.0, 0.0}; } + double saturatedInput = (greaterInput + lesserInput) / greaterInput; + leftSpeed /= saturatedInput; + rightSpeed /= saturatedInput; return {leftSpeed, rightSpeed}; } @@ -144,14 +124,14 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::CurvatureDriveIK( double rightSpeed = 0.0; if (allowTurnInPlace) { - leftSpeed = xSpeed + zRotation; - rightSpeed = xSpeed - zRotation; + leftSpeed = xSpeed - zRotation; + rightSpeed = xSpeed + zRotation; } else { - leftSpeed = xSpeed + std::abs(xSpeed) * zRotation; - rightSpeed = xSpeed - std::abs(xSpeed) * zRotation; + leftSpeed = xSpeed - std::abs(xSpeed) * zRotation; + rightSpeed = xSpeed + std::abs(xSpeed) * zRotation; } - // Normalize wheel speeds + // Desaturate wheel speeds double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed)); if (maxMagnitude > 1.0) { leftSpeed /= maxMagnitude; diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp deleted file mode 100644 index ae6e118d9f..0000000000 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// 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 "frc/drive/KilloughDrive.h" - -#include -#include -#include - -#include -#include -#include - -#include "frc/MathUtil.h" -#include "frc/motorcontrol/MotorController.h" - -using namespace frc; - -KilloughDrive::KilloughDrive(MotorController& leftMotor, - MotorController& rightMotor, - MotorController& backMotor) - : KilloughDrive(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle, - kDefaultRightMotorAngle, kDefaultBackMotorAngle) {} - -KilloughDrive::KilloughDrive(MotorController& leftMotor, - MotorController& rightMotor, - MotorController& backMotor, double leftMotorAngle, - double rightMotorAngle, double backMotorAngle) - : m_leftMotor(&leftMotor), - m_rightMotor(&rightMotor), - m_backMotor(&backMotor) { - m_leftVec = {std::cos(leftMotorAngle * (std::numbers::pi / 180.0)), - std::sin(leftMotorAngle * (std::numbers::pi / 180.0))}; - m_rightVec = {std::cos(rightMotorAngle * (std::numbers::pi / 180.0)), - std::sin(rightMotorAngle * (std::numbers::pi / 180.0))}; - m_backVec = {std::cos(backMotorAngle * (std::numbers::pi / 180.0)), - std::sin(backMotorAngle * (std::numbers::pi / 180.0))}; - wpi::SendableRegistry::AddChild(this, m_leftMotor); - wpi::SendableRegistry::AddChild(this, m_rightMotor); - wpi::SendableRegistry::AddChild(this, m_backMotor); - static int instances = 0; - ++instances; - wpi::SendableRegistry::AddLW(this, "KilloughDrive", instances); -} - -void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed, - double zRotation, double gyroAngle) { - if (!reported) { - HAL_Report(HALUsageReporting::kResourceType_RobotDrive, - HALUsageReporting::kRobotDrive2_KilloughCartesian, 3); - reported = true; - } - - ySpeed = ApplyDeadband(ySpeed, m_deadband); - xSpeed = ApplyDeadband(xSpeed, m_deadband); - - auto [left, right, back] = - DriveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); - - m_leftMotor->Set(left * m_maxOutput); - m_rightMotor->Set(right * m_maxOutput); - m_backMotor->Set(back * m_maxOutput); - - Feed(); -} - -void KilloughDrive::DrivePolar(double magnitude, double angle, - double zRotation) { - if (!reported) { - HAL_Report(HALUsageReporting::kResourceType_RobotDrive, - HALUsageReporting::kRobotDrive2_KilloughPolar, 3); - reported = true; - } - - DriveCartesian(magnitude * std::sin(angle * (std::numbers::pi / 180.0)), - magnitude * std::cos(angle * (std::numbers::pi / 180.0)), - 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; - - Desaturate(wheelSpeeds); - - return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]}; -} - -void KilloughDrive::StopMotor() { - m_leftMotor->StopMotor(); - m_rightMotor->StopMotor(); - m_backMotor->StopMotor(); - Feed(); -} - -std::string KilloughDrive::GetDescription() const { - return "KilloughDrive"; -} - -void KilloughDrive::InitSendable(wpi::SendableBuilder& builder) { - builder.SetSmartDashboardType("KilloughDrive"); - builder.SetActuator(true); - builder.SetSafeState([=, this] { StopMotor(); }); - builder.AddDoubleProperty( - "Left Motor Speed", [=, this] { return m_leftMotor->Get(); }, - [=, this](double value) { m_leftMotor->Set(value); }); - builder.AddDoubleProperty( - "Right Motor Speed", [=, this] { return m_rightMotor->Get(); }, - [=, this](double value) { m_rightMotor->Set(value); }); - builder.AddDoubleProperty( - "Back Motor Speed", [=, this] { return m_backMotor->Get(); }, - [=, this](double value) { m_backMotor->Set(value); }); -} diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index b714cd2dfc..2bf6a3f3c4 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -5,15 +5,13 @@ #include "frc/drive/MecanumDrive.h" #include -#include -#include #include #include #include #include "frc/MathUtil.h" -#include "frc/drive/Vector2d.h" +#include "frc/geometry/Translation2d.h" #include "frc/motorcontrol/MotorController.h" using namespace frc; @@ -35,19 +33,19 @@ MecanumDrive::MecanumDrive(MotorController& frontLeftMotor, wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances); } -void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed, - double zRotation, double gyroAngle) { +void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed, + double zRotation, Rotation2d gyroAngle) { if (!reported) { HAL_Report(HALUsageReporting::kResourceType_RobotDrive, HALUsageReporting::kRobotDrive2_MecanumCartesian, 4); reported = true; } - ySpeed = ApplyDeadband(ySpeed, m_deadband); xSpeed = ApplyDeadband(xSpeed, m_deadband); + ySpeed = ApplyDeadband(ySpeed, m_deadband); auto [frontLeft, frontRight, rearLeft, rearRight] = - DriveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); + DriveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle); m_frontLeftMotor->Set(frontLeft * m_maxOutput); m_frontRightMotor->Set(frontRight * m_maxOutput); @@ -57,7 +55,7 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed, Feed(); } -void MecanumDrive::DrivePolar(double magnitude, double angle, +void MecanumDrive::DrivePolar(double magnitude, Rotation2d angle, double zRotation) { if (!reported) { HAL_Report(HALUsageReporting::kResourceType_RobotDrive, @@ -65,9 +63,8 @@ void MecanumDrive::DrivePolar(double magnitude, double angle, reported = true; } - DriveCartesian(magnitude * std::cos(angle * (std::numbers::pi / 180.0)), - magnitude * std::sin(angle * (std::numbers::pi / 180.0)), - zRotation, 0.0); + DriveCartesian(magnitude * angle.Cos(), magnitude * angle.Sin(), zRotation, + 0_rad); } void MecanumDrive::StopMotor() { @@ -78,22 +75,23 @@ void MecanumDrive::StopMotor() { Feed(); } -MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double ySpeed, - double xSpeed, +MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double xSpeed, + double ySpeed, double zRotation, - double gyroAngle) { - ySpeed = std::clamp(ySpeed, -1.0, 1.0); + Rotation2d gyroAngle) { xSpeed = std::clamp(xSpeed, -1.0, 1.0); + ySpeed = std::clamp(ySpeed, -1.0, 1.0); // Compensate for gyro angle. - Vector2d input{ySpeed, xSpeed}; - input.Rotate(-gyroAngle); + auto input = + Translation2d{units::meter_t{xSpeed}, units::meter_t{ySpeed}}.RotateBy( + -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; + wheelSpeeds[kFrontLeft] = input.X().value() + input.Y().value() + zRotation; + wheelSpeeds[kFrontRight] = input.X().value() - input.Y().value() - zRotation; + wheelSpeeds[kRearLeft] = input.X().value() - input.Y().value() + zRotation; + wheelSpeeds[kRearRight] = input.X().value() + input.Y().value() - zRotation; Desaturate(wheelSpeeds); diff --git a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp deleted file mode 100644 index acfbbd60b2..0000000000 --- a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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 "frc/drive/Vector2d.h" - -#include -#include - -using namespace frc; - -Vector2d::Vector2d(double x, double y) { - this->x = x; - this->y = y; -} - -void Vector2d::Rotate(double angle) { - double cosA = std::cos(angle * (std::numbers::pi / 180.0)); - double sinA = std::sin(angle * (std::numbers::pi / 180.0)); - double out[2]; - out[0] = x * cosA - y * sinA; - out[1] = x * sinA + y * cosA; - x = out[0]; - y = out[1]; -} - -double Vector2d::Dot(const Vector2d& vec) const { - return x * vec.x + y * vec.y; -} - -double Vector2d::Magnitude() const { - return std::sqrt(x * x + y * y); -} - -double Vector2d::ScalarProject(const Vector2d& vec) const { - return Dot(vec) / vec.Magnitude(); -} diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h index 6ce60b66cd..2e701a3817 100644 --- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h @@ -73,13 +73,11 @@ class MotorController; * Each drive function provides different inverse kinematic relations for a * differential drive robot. * - * This library uses the NED axes convention (North-East-Down as external - * reference in the world frame): - * http://www.nuclearprojects.com/ins/images/axis_big.png. - * - * The positive X axis points ahead, the positive Y axis points to the right, - * and the positive Z axis points down. Rotations follow the right-hand rule, so - * clockwise rotation around the Z axis is positive. + * This library uses the NWU axes convention (North-West-Up as external + * reference in the world frame). The positive X axis points ahead, the positive + * Y axis points to the left, and the positive Z axis points up. Rotations + * follow the right-hand rule, so counterclockwise rotation around the Z axis is + * positive. * * 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 @@ -125,7 +123,7 @@ class DifferentialDrive : public RobotDriveBase, * @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. + * [-1.0..1.0]. Counterclockwise is positive. * @param squareInputs If set, decreases the input sensitivity at low speeds. */ void ArcadeDrive(double xSpeed, double zRotation, bool squareInputs = true); @@ -139,8 +137,8 @@ class DifferentialDrive : public RobotDriveBase, * * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. * Forward is positive. - * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is - * positive. + * @param zRotation The normalized curvature [-1.0..1.0]. + * Counterclockwise is positive. * @param allowTurnInPlace If set, overrides constant-curvature turning for * turn-in-place maneuvers. zRotation will control * turning rate instead of curvature. diff --git a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h deleted file mode 100644 index df39aac51e..0000000000 --- a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h +++ /dev/null @@ -1,176 +0,0 @@ -// 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. - -#pragma once - -#include -#include - -#include -#include - -#include "frc/drive/RobotDriveBase.h" -#include "frc/drive/Vector2d.h" - -namespace frc { - -class MotorController; - -/** - * A class for driving Killough drive platforms. - * - * Killough drives are triangular with one omni wheel on each corner. - * - * Drive base diagram: - * 
- *  /_____\
- * / \   / \
- *    \ /
- *    ---
- *
- * - * Each Drive() function provides different inverse kinematic relations for a - * Killough drive. The default wheel vectors are parallel to their respective - * opposite sides, but can be overridden. See the constructor for more - * information. - * - * This library uses the NED axes convention (North-East-Down as external - * reference in the world frame): - * http://www.nuclearprojects.com/ins/images/axis_big.png. - * - * The positive X axis points ahead, the positive Y axis points right, and the - * and the positive Z axis points down. Rotations follow the right-hand rule, so - * clockwise rotation around the Z axis is positive. - * - * MotorSafety is enabled by default. The DriveCartesian or DrivePolar - * methods should be called periodically to avoid Motor Safety timeouts. - */ -class KilloughDrive : public RobotDriveBase, - public wpi::Sendable, - public wpi::SendableHelper { - public: - static constexpr double kDefaultLeftMotorAngle = 60.0; - static constexpr double kDefaultRightMotorAngle = 120.0; - static constexpr double kDefaultBackMotorAngle = 270.0; - - /** - * Wheel speeds for a Killough drive. - * - * Uses normalized voltage [-1.0..1.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. - * - * The default motor angles make the wheels on each corner parallel to their - * respective opposite sides. - * - * If a motor needs to be inverted, do so before passing it in. - * - * @param leftMotor The motor on the left corner. - * @param rightMotor The motor on the right corner. - * @param backMotor The motor on the back corner. - */ - KilloughDrive(MotorController& leftMotor, MotorController& rightMotor, - MotorController& backMotor); - - /** - * Construct a Killough drive with the given motors. - * - * Angles are measured in degrees clockwise from the positive X axis. - * - * @param leftMotor The motor on the left corner. - * @param rightMotor The motor on the right corner. - * @param backMotor The motor on the back corner. - * @param leftMotorAngle The angle of the left wheel's forward direction of - * travel. - * @param rightMotorAngle The angle of the right wheel's forward direction of - * travel. - * @param backMotorAngle The angle of the back wheel's forward direction of - * travel. - */ - KilloughDrive(MotorController& leftMotor, MotorController& rightMotor, - MotorController& backMotor, double leftMotorAngle, - double rightMotorAngle, double backMotorAngle); - - ~KilloughDrive() override = default; - - KilloughDrive(KilloughDrive&&) = default; - KilloughDrive& operator=(KilloughDrive&&) = default; - - /** - * Drive method 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. - */ - void DriveCartesian(double ySpeed, double xSpeed, double zRotation, - double gyroAngle = 0.0); - - /** - * Drive method 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 magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is - * positive. - * @param angle The angle around the Z axis at which the robot drives in - * degrees [-180..180]. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. - * Clockwise is positive. - */ - 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. - * @return Wheel speeds [-1.0..1.0]. - */ - WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation, - double gyroAngle = 0.0); - - void StopMotor() override; - std::string GetDescription() const override; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - MotorController* m_leftMotor; - MotorController* m_rightMotor; - MotorController* m_backMotor; - - Vector2d m_leftVec; - Vector2d m_rightVec; - Vector2d m_backVec; - - bool reported = false; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h index 282453cef1..f4c28f423d 100644 --- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h @@ -7,10 +7,12 @@ #include #include +#include #include #include #include "frc/drive/RobotDriveBase.h" +#include "frc/geometry/Rotation2d.h" namespace frc { @@ -35,9 +37,11 @@ class MotorController; * Each Drive() function provides different inverse kinematic relations for a * Mecanum drive robot. * - * The positive Y axis points ahead, the positive X axis points to the right, - * and the positive Z axis points down. Rotations follow the right-hand rule, so - * clockwise rotation around the Z axis is positive. + * This library uses the NWU axes convention (North-West-Up as external + * reference in the world frame). The positive X axis points ahead, the positive + * Y axis points to the left, and the positive Z axis points up. Rotations + * follow the right-hand rule, so counterclockwise rotation around the Z axis is + * positive. * * Note: the axis conventions used in this class differ from DifferentialDrive. * This may change in a future year's WPILib release. @@ -82,54 +86,54 @@ class MecanumDrive : public RobotDriveBase, /** * Drive method for Mecanum platform. * - * Angles are measured clockwise from the positive X axis. The robot's speed - * is independent from its angle or rotation rate. + * Angles are measured counterclockwise 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]. Forward is + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is * positive. - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left 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. + * Counterclockwise is positive. + * @param gyroAngle The gyro heading around the Z axis. Use this to implement + * field-oriented controls. */ - void DriveCartesian(double ySpeed, double xSpeed, double zRotation, - double gyroAngle = 0.0); + void DriveCartesian(double xSpeed, double ySpeed, double zRotation, + Rotation2d gyroAngle = 0_rad); /** * Drive method for Mecanum platform. * - * Angles are measured clockwise from the positive X axis. The robot's speed - * is independent from its angle or rotation rate. + * Angles are measured counterclockwise from the positive X axis. The robot's + * speed is independent from its angle or rotation rate. * * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is * positive. - * @param angle The angle around the Z axis at which the robot drives in - * degrees [-180..180]. + * @param angle The angle around the Z axis at which the robot drives. * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. - * Clockwise is positive. + * Counterclockwise is positive. */ - void DrivePolar(double magnitude, double angle, double zRotation); + void DrivePolar(double magnitude, Rotation2d angle, double zRotation); /** * Cartesian inverse kinematics for Mecanum platform. * - * Angles are measured clockwise from the positive X axis. The robot's speed - * is independent from its angle or rotation rate. + * Angles are measured counterclockwise 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]. Forward is + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is * positive. - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left 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. + * Counterclockwise is positive. + * @param gyroAngle The gyro heading around the Z axis. Use this to implement + * field-oriented controls. * @return Wheel speeds [-1.0..1.0]. */ - static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, - double zRotation, double gyroAngle = 0.0); + static WheelSpeeds DriveCartesianIK(double xSpeed, double ySpeed, + double zRotation, + Rotation2d gyroAngle = 0_rad); void StopMotor() override; std::string GetDescription() const override; diff --git a/wpilibc/src/main/native/include/frc/drive/Vector2d.h b/wpilibc/src/main/native/include/frc/drive/Vector2d.h deleted file mode 100644 index 92a3de6fe1..0000000000 --- a/wpilibc/src/main/native/include/frc/drive/Vector2d.h +++ /dev/null @@ -1,46 +0,0 @@ -// 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. - -#pragma once - -namespace frc { - -/** - * This is a 2D vector struct that supports basic vector operations. - */ -struct Vector2d { - Vector2d() = default; - Vector2d(double x, double y); - - /** - * Rotate a vector in Cartesian space. - * - * @param angle angle in degrees by which to rotate vector counter-clockwise. - */ - void Rotate(double angle); - - /** - * Returns dot product of this vector with argument. - * - * @param vec Vector with which to perform dot product. - */ - double Dot(const Vector2d& vec) const; - - /** - * Returns magnitude of vector. - */ - double Magnitude() const; - - /** - * Returns scalar projection of this vector onto argument. - * - * @param vec Vector onto which to project this vector. - */ - double ScalarProject(const Vector2d& vec) const; - - double x = 0.0; - double y = 0.0; -}; - -} // namespace frc diff --git a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp index 7358c8d244..1f542be14f 100644 --- a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp +++ b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp @@ -13,12 +13,12 @@ TEST(DifferentialDriveTest, ArcadeDriveIK) { EXPECT_DOUBLE_EQ(1.0, speeds.right); // Forward left turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false); EXPECT_DOUBLE_EQ(0.0, speeds.left); EXPECT_DOUBLE_EQ(0.5, speeds.right); // Forward right turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false); EXPECT_DOUBLE_EQ(0.5, speeds.left); EXPECT_DOUBLE_EQ(0.0, speeds.right); @@ -28,32 +28,32 @@ TEST(DifferentialDriveTest, ArcadeDriveIK) { EXPECT_DOUBLE_EQ(-1.0, speeds.right); // Backward left turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false); EXPECT_DOUBLE_EQ(-0.5, speeds.left); EXPECT_DOUBLE_EQ(0.0, speeds.right); // Backward right turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false); EXPECT_DOUBLE_EQ(0.0, speeds.left); EXPECT_DOUBLE_EQ(-0.5, speeds.right); // Left turn (xSpeed with negative sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false); EXPECT_DOUBLE_EQ(-1.0, speeds.left); EXPECT_DOUBLE_EQ(1.0, speeds.right); // Left turn (xSpeed with positive sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false); EXPECT_DOUBLE_EQ(-1.0, speeds.left); EXPECT_DOUBLE_EQ(1.0, speeds.right); // Right turn (xSpeed with negative sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false); EXPECT_DOUBLE_EQ(1.0, speeds.left); EXPECT_DOUBLE_EQ(-1.0, speeds.right); // Right turn (xSpeed with positive sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false); EXPECT_DOUBLE_EQ(1.0, speeds.left); EXPECT_DOUBLE_EQ(-1.0, speeds.right); } @@ -65,12 +65,12 @@ TEST(DifferentialDriveTest, ArcadeDriveIKSquared) { EXPECT_DOUBLE_EQ(1.0, speeds.right); // Forward left turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true); EXPECT_DOUBLE_EQ(0.0, speeds.left); EXPECT_DOUBLE_EQ(0.25, speeds.right); // Forward right turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true); EXPECT_DOUBLE_EQ(0.25, speeds.left); EXPECT_DOUBLE_EQ(0.0, speeds.right); @@ -80,32 +80,32 @@ TEST(DifferentialDriveTest, ArcadeDriveIKSquared) { EXPECT_DOUBLE_EQ(-1.0, speeds.right); // Backward left turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true); EXPECT_DOUBLE_EQ(-0.25, speeds.left); EXPECT_DOUBLE_EQ(0.0, speeds.right); // Backward right turn - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true); EXPECT_DOUBLE_EQ(0.0, speeds.left); EXPECT_DOUBLE_EQ(-0.25, speeds.right); // Left turn (xSpeed with negative sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false); EXPECT_DOUBLE_EQ(-1.0, speeds.left); EXPECT_DOUBLE_EQ(1.0, speeds.right); // Left turn (xSpeed with positive sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false); EXPECT_DOUBLE_EQ(-1.0, speeds.left); EXPECT_DOUBLE_EQ(1.0, speeds.right); // Right turn (xSpeed with negative sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false); EXPECT_DOUBLE_EQ(1.0, speeds.left); EXPECT_DOUBLE_EQ(-1.0, speeds.right); // Right turn (xSpeed with positive sign) - speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false); + speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false); EXPECT_DOUBLE_EQ(1.0, speeds.left); EXPECT_DOUBLE_EQ(-1.0, speeds.right); } @@ -117,12 +117,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIK) { EXPECT_DOUBLE_EQ(1.0, speeds.right); // Forward left turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false); + speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false); EXPECT_DOUBLE_EQ(0.25, speeds.left); EXPECT_DOUBLE_EQ(0.75, speeds.right); // Forward right turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false); + speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false); EXPECT_DOUBLE_EQ(0.75, speeds.left); EXPECT_DOUBLE_EQ(0.25, speeds.right); @@ -132,12 +132,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIK) { EXPECT_DOUBLE_EQ(-1.0, speeds.right); // Backward left turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false); + speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false); EXPECT_DOUBLE_EQ(-0.75, speeds.left); EXPECT_DOUBLE_EQ(-0.25, speeds.right); // Backward right turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false); + speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false); EXPECT_DOUBLE_EQ(-0.25, speeds.left); EXPECT_DOUBLE_EQ(-0.75, speeds.right); } @@ -149,12 +149,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) { EXPECT_DOUBLE_EQ(1.0, speeds.right); // Forward left turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true); + speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true); EXPECT_DOUBLE_EQ(0.0, speeds.left); EXPECT_DOUBLE_EQ(1.0, speeds.right); // Forward right turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true); + speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true); EXPECT_DOUBLE_EQ(1.0, speeds.left); EXPECT_DOUBLE_EQ(0.0, speeds.right); @@ -164,12 +164,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) { EXPECT_DOUBLE_EQ(-1.0, speeds.right); // Backward left turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true); + speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true); EXPECT_DOUBLE_EQ(-1.0, speeds.left); EXPECT_DOUBLE_EQ(0.0, speeds.right); // Backward right turn - speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true); + speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true); EXPECT_DOUBLE_EQ(0.0, speeds.left); EXPECT_DOUBLE_EQ(-1.0, speeds.right); } @@ -250,12 +250,12 @@ TEST(DifferentialDriveTest, ArcadeDrive) { EXPECT_DOUBLE_EQ(1.0, right.Get()); // Forward left turn - drive.ArcadeDrive(0.5, -0.5, false); + 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); + drive.ArcadeDrive(0.5, -0.5, false); EXPECT_DOUBLE_EQ(0.5, left.Get()); EXPECT_DOUBLE_EQ(0.0, right.Get()); @@ -265,12 +265,12 @@ TEST(DifferentialDriveTest, ArcadeDrive) { EXPECT_DOUBLE_EQ(-1.0, right.Get()); // Backward left turn - drive.ArcadeDrive(-0.5, -0.5, false); + 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); + drive.ArcadeDrive(-0.5, -0.5, false); EXPECT_DOUBLE_EQ(0.0, left.Get()); EXPECT_DOUBLE_EQ(-0.5, right.Get()); } @@ -287,12 +287,12 @@ TEST(DifferentialDriveTest, ArcadeDriveSquared) { EXPECT_DOUBLE_EQ(1.0, right.Get()); // Forward left turn - drive.ArcadeDrive(0.5, -0.5, true); + 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); + drive.ArcadeDrive(0.5, -0.5, true); EXPECT_DOUBLE_EQ(0.25, left.Get()); EXPECT_DOUBLE_EQ(0.0, right.Get()); @@ -302,12 +302,12 @@ TEST(DifferentialDriveTest, ArcadeDriveSquared) { EXPECT_DOUBLE_EQ(-1.0, right.Get()); // Backward left turn - drive.ArcadeDrive(-0.5, -0.5, true); + 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); + drive.ArcadeDrive(-0.5, -0.5, true); EXPECT_DOUBLE_EQ(0.0, left.Get()); EXPECT_DOUBLE_EQ(-0.25, right.Get()); } @@ -324,12 +324,12 @@ TEST(DifferentialDriveTest, CurvatureDrive) { EXPECT_DOUBLE_EQ(1.0, right.Get()); // Forward left turn - drive.CurvatureDrive(0.5, -0.5, false); + 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); + drive.CurvatureDrive(0.5, -0.5, false); EXPECT_DOUBLE_EQ(0.75, left.Get()); EXPECT_DOUBLE_EQ(0.25, right.Get()); @@ -339,12 +339,12 @@ TEST(DifferentialDriveTest, CurvatureDrive) { EXPECT_DOUBLE_EQ(-1.0, right.Get()); // Backward left turn - drive.CurvatureDrive(-0.5, -0.5, false); + 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); + drive.CurvatureDrive(-0.5, -0.5, false); EXPECT_DOUBLE_EQ(-0.25, left.Get()); EXPECT_DOUBLE_EQ(-0.75, right.Get()); } @@ -361,12 +361,12 @@ TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) { EXPECT_DOUBLE_EQ(1.0, right.Get()); // Forward left turn - drive.CurvatureDrive(0.5, -0.5, true); + 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); + drive.CurvatureDrive(0.5, -0.5, true); EXPECT_DOUBLE_EQ(1.0, left.Get()); EXPECT_DOUBLE_EQ(0.0, right.Get()); @@ -376,12 +376,12 @@ TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) { EXPECT_DOUBLE_EQ(-1.0, right.Get()); // Backward left turn - drive.CurvatureDrive(-0.5, -0.5, true); + 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); + drive.CurvatureDrive(-0.5, -0.5, true); EXPECT_DOUBLE_EQ(0.0, left.Get()); EXPECT_DOUBLE_EQ(-1.0, right.Get()); } diff --git a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp deleted file mode 100644 index 8cec8d3663..0000000000 --- a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// 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 "frc/drive/KilloughDrive.h" -#include "gtest/gtest.h" -#include "motorcontrol/MockMotorController.h" - -TEST(KilloughDriveTest, CartesianIK) { - frc::MockMotorController left; - frc::MockMotorController right; - frc::MockMotorController back; - frc::KilloughDrive drive{left, right, back}; - - // Forward - auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0); - EXPECT_DOUBLE_EQ(0.5, speeds.left); - EXPECT_DOUBLE_EQ(-0.5, speeds.right); - EXPECT_NEAR(0.0, speeds.back, 1e-9); - - // Left - speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0); - EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left); - EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right); - EXPECT_DOUBLE_EQ(1.0, speeds.back); - - // Right - speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0); - EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.left); - EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.right); - EXPECT_DOUBLE_EQ(-1.0, speeds.back); - - // Rotate CCW - speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0); - EXPECT_DOUBLE_EQ(-1.0, speeds.left); - EXPECT_DOUBLE_EQ(-1.0, speeds.right); - EXPECT_DOUBLE_EQ(-1.0, speeds.back); - - // Rotate CW - speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0); - EXPECT_DOUBLE_EQ(1.0, speeds.left); - EXPECT_DOUBLE_EQ(1.0, speeds.right); - EXPECT_DOUBLE_EQ(1.0, speeds.back); -} - -TEST(KilloughDriveTest, CartesianIKGyro90CW) { - frc::MockMotorController left; - frc::MockMotorController right; - frc::MockMotorController back; - frc::KilloughDrive drive{left, right, back}; - - // Forward in global frame; left in robot frame - auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0, 90.0); - EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left); - EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right); - EXPECT_DOUBLE_EQ(1.0, speeds.back); - - // Left in global frame; backward in robot frame - speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0, 90.0); - EXPECT_DOUBLE_EQ(-0.5, speeds.left); - EXPECT_NEAR(0.5, speeds.right, 1e-9); - EXPECT_NEAR(0.0, speeds.back, 1e-9); - - // Right in global frame; forward in robot frame - speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0, 90.0); - EXPECT_DOUBLE_EQ(0.5, speeds.left); - EXPECT_NEAR(-0.5, speeds.right, 1e-9); - EXPECT_NEAR(0.0, speeds.back, 1e-9); - - // Rotate CCW - speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0, 90.0); - EXPECT_DOUBLE_EQ(-1.0, speeds.left); - EXPECT_DOUBLE_EQ(-1.0, speeds.right); - EXPECT_DOUBLE_EQ(-1.0, speeds.back); - - // Rotate CW - speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0, 90.0); - EXPECT_DOUBLE_EQ(1.0, speeds.left); - EXPECT_DOUBLE_EQ(1.0, speeds.right); - EXPECT_DOUBLE_EQ(1.0, speeds.back); -} - -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 index 68d4a27e21..de554625df 100644 --- a/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp +++ b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp @@ -45,35 +45,35 @@ TEST(MecanumDriveTest, CartesianIK) { TEST(MecanumDriveTest, CartesianIKGyro90CW) { // Forward in global frame; left in robot frame - auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90.0); + auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90_deg); EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft); EXPECT_DOUBLE_EQ(1.0, speeds.frontRight); EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft); EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight); // Left in global frame; backward in robot frame - speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90.0); + speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90_deg); EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft); EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight); EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft); EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight); // Right in global frame; forward in robot frame - speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90.0); + speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90_deg); EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft); EXPECT_DOUBLE_EQ(1.0, speeds.frontRight); EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft); EXPECT_DOUBLE_EQ(1.0, speeds.rearRight); // Rotate CCW - speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90.0); + speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90_deg); EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft); EXPECT_DOUBLE_EQ(1.0, speeds.frontRight); EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft); EXPECT_DOUBLE_EQ(1.0, speeds.rearRight); // Rotate CW - speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90.0); + speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90_deg); EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft); EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight); EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft); @@ -133,35 +133,35 @@ TEST(MecanumDriveTest, CartesianGyro90CW) { drive.SetDeadband(0.0); // Forward in global frame; left in robot frame - drive.DriveCartesian(1.0, 0.0, 0.0, 90.0); + drive.DriveCartesian(1.0, 0.0, 0.0, 90_deg); 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); + drive.DriveCartesian(0.0, -1.0, 0.0, 90_deg); 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); + drive.DriveCartesian(0.0, 1.0, 0.0, 90_deg); 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); + drive.DriveCartesian(0.0, 0.0, -1.0, 90_deg); 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); + drive.DriveCartesian(0.0, 0.0, 1.0, 90_deg); EXPECT_DOUBLE_EQ(1.0, fl.Get()); EXPECT_DOUBLE_EQ(-1.0, fr.Get()); EXPECT_DOUBLE_EQ(1.0, rl.Get()); @@ -177,35 +177,35 @@ TEST(MecanumDriveTest, Polar) { drive.SetDeadband(0.0); // Forward - drive.DrivePolar(1.0, 0.0, 0.0); + drive.DrivePolar(1.0, 0_deg, 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); + drive.DrivePolar(1.0, -90_deg, 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); + drive.DrivePolar(1.0, 90_deg, 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); + drive.DrivePolar(0.0, 0_deg, -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); + drive.DrivePolar(0.0, 0_deg, 1.0); EXPECT_DOUBLE_EQ(1.0, fl.Get()); EXPECT_DOUBLE_EQ(-1.0, fr.Get()); EXPECT_DOUBLE_EQ(1.0, rl.Get()); diff --git a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp index a4e0457891..176dc1e1fc 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp @@ -29,7 +29,7 @@ class Robot : public frc::TimedRobot { */ void TeleopPeriodic() override { m_robotDrive.DriveCartesian(-m_joystick.GetY(), m_joystick.GetX(), - m_joystick.GetZ(), m_gyro.GetAngle()); + m_joystick.GetZ(), m_gyro.GetRotation2d()); } private: diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 9e4018354e..417368415d 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -50,7 +50,7 @@ void DriveSubsystem::Periodic() { void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot, bool fieldRelative) { if (fieldRelative) { - m_drive.DriveCartesian(ySpeed, xSpeed, rot, -m_gyro.GetAngle()); + m_drive.DriveCartesian(ySpeed, xSpeed, rot, m_gyro.GetRotation2d()); } else { m_drive.DriveCartesian(ySpeed, xSpeed, rot); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp index f8d95acb19..6f70e20cca 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp @@ -21,7 +21,7 @@ class Robot : public frc::TimedRobot { } void TeleopPeriodic() override { - /* Use the joystick X axis for lateral movement, Y axis for forward + /* Use the joystick X axis for forward movement, Y axis for lateral * movement, and Z axis for rotation. */ m_robotDrive.DriveCartesian(-m_stick.GetY(), m_stick.GetX(), 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 fe846b6a46..8db30d3aff 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 @@ -73,12 +73,10 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController; *

Each drive function provides different inverse kinematic relations for a differential drive * robot. * - *

This library uses the NED axes convention (North-East-Down as external reference in the world - * frame): http://www.nuclearprojects.com/ins/images/axis_big.png. - * - *

The positive X axis points ahead, the positive Y axis points right, and the positive Z axis - * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is - * positive. + *

This library uses the NWU axes convention (North-West-Up as external reference in the world + * frame). The positive X axis points ahead, the positive Y axis points to the left, and the + * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation + * around the Z axis is positive. * *

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 @@ -152,7 +150,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * decrease sensitivity at low speeds. * * @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 + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. */ public void arcadeDrive(double xSpeed, double zRotation) { @@ -163,7 +161,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * Arcade drive method 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 + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. * @param squareInputs If set, decreases the input sensitivity at low speeds. */ @@ -192,7 +190,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * heading change. This makes the robot more controllable at high speeds. * * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. - * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive. + * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive. * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place * maneuvers. zRotation will control turning rate instead of curvature. */ @@ -256,7 +254,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * 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 + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. * @param squareInputs If set, decreases the input sensitivity at low speeds. * @return Wheel speeds [-1.0..1.0]. @@ -272,37 +270,19 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC zRotation = Math.copySign(zRotation * zRotation, zRotation); } - double leftSpeed; - double rightSpeed; + double leftSpeed = xSpeed - zRotation; + double rightSpeed = xSpeed + zRotation; - double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed); - - if (Double.compare(xSpeed, 0.0) >= 0) { - // First quadrant, else second quadrant - if (Double.compare(zRotation, 0.0) >= 0) { - leftSpeed = maxInput; - rightSpeed = xSpeed - zRotation; - } else { - leftSpeed = xSpeed + zRotation; - rightSpeed = maxInput; - } - } else { - // Third quadrant, else fourth quadrant - if (Double.compare(zRotation, 0.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; + // Find the maximum possible value of (throttle + turn) along the vector + // that the joystick is pointing, then desaturate the wheel speeds + double greaterInput = Math.max(Math.abs(xSpeed), Math.abs(zRotation)); + double lesserInput = Math.min(Math.abs(xSpeed), Math.abs(zRotation)); + if (greaterInput == 0.0) { + return new WheelSpeeds(0.0, 0.0); } + double saturatedInput = (greaterInput + lesserInput) / greaterInput; + leftSpeed /= saturatedInput; + rightSpeed /= saturatedInput; return new WheelSpeeds(leftSpeed, rightSpeed); } @@ -314,7 +294,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC * heading change. This makes the robot more controllable at high speeds. * * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. - * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive. + * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive. * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place * maneuvers. zRotation will control rotation rate around the Z axis instead of curvature. * @return Wheel speeds [-1.0..1.0]. @@ -328,14 +308,14 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC double rightSpeed; if (allowTurnInPlace) { - leftSpeed = xSpeed + zRotation; - rightSpeed = xSpeed - zRotation; + leftSpeed = xSpeed - zRotation; + rightSpeed = xSpeed + zRotation; } else { - leftSpeed = xSpeed + Math.abs(xSpeed) * zRotation; - rightSpeed = xSpeed - Math.abs(xSpeed) * zRotation; + leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation; + rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation; } - // Normalize wheel speeds + // Desaturate wheel speeds double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); if (maxMagnitude > 1.0) { leftSpeed /= maxMagnitude; 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 deleted file mode 100644 index d84384fd67..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java +++ /dev/null @@ -1,309 +0,0 @@ -// 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 java.util.Objects.requireNonNull; - -import edu.wpi.first.hal.FRCNetComm.tInstances; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; - -/** - * A class for driving Killough drive platforms. - * - *

Killough drives are triangular with one omni wheel on each corner. - * - *

Drive base diagram: - * - * 

- *  /_____\
- * / \   / \
- *    \ /
- *    ---
- *
- * - *

Each drive() function provides different inverse kinematic relations for a Killough drive. The - * default wheel vectors are parallel to their respective opposite sides, but can be overridden. See - * the constructor for more information. - * - *

This library uses the NED axes convention (North-East-Down as external reference in the world - * frame): http://www.nuclearprojects.com/ins/images/axis_big.png. - * - *

The positive X axis points ahead, the positive Y axis points right, and the positive Z axis - * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is - * positive. - * - *

{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The driveCartesian or - * drivePolar methods should be called periodically to avoid Motor Safety timeouts. - */ -public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable { - public static final double kDefaultLeftMotorAngle = 60.0; - public static final double kDefaultRightMotorAngle = 120.0; - public static final double kDefaultBackMotorAngle = 270.0; - - private static int instances; - - private MotorController m_leftMotor; - private MotorController m_rightMotor; - private MotorController m_backMotor; - - private Vector2d m_leftVec; - private Vector2d m_rightVec; - private Vector2d m_backVec; - - private boolean m_reported; - - /** - * Wheel speeds for a Killough drive. - * - *

Uses normalized voltage [-1.0..1.0]. - */ - @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 [-1.0..1.0]. - * @param right The right speed [-1.0..1.0]. - * @param back The back speed [-1.0..1.0]. - */ - 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. - * - *

The default motor angles make the wheels on each corner parallel to their respective - * opposite sides. - * - *

If a motor needs to be inverted, do so before passing it in. - * - * @param leftMotor The motor on the left corner. - * @param rightMotor The motor on the right corner. - * @param backMotor The motor on the back corner. - */ - public KilloughDrive( - MotorController leftMotor, MotorController rightMotor, MotorController backMotor) { - this( - leftMotor, - rightMotor, - backMotor, - kDefaultLeftMotorAngle, - kDefaultRightMotorAngle, - kDefaultBackMotorAngle); - } - - /** - * Construct a Killough drive with the given motors. - * - *

Angles are measured in degrees clockwise from the positive X axis. - * - * @param leftMotor The motor on the left corner. - * @param rightMotor The motor on the right corner. - * @param backMotor The motor on the back corner. - * @param leftMotorAngle The angle of the left wheel's forward direction of travel. - * @param rightMotorAngle The angle of the right wheel's forward direction of travel. - * @param backMotorAngle The angle of the back wheel's forward direction of travel. - */ - public KilloughDrive( - MotorController leftMotor, - MotorController rightMotor, - MotorController backMotor, - double leftMotorAngle, - double rightMotorAngle, - double backMotorAngle) { - requireNonNull(leftMotor, "Left motor cannot be null"); - requireNonNull(rightMotor, "Right motor cannot be null"); - requireNonNull(backMotor, "Back motor cannot be null"); - - m_leftMotor = leftMotor; - m_rightMotor = rightMotor; - m_backMotor = backMotor; - m_leftVec = - new Vector2d( - Math.cos(leftMotorAngle * (Math.PI / 180.0)), - Math.sin(leftMotorAngle * (Math.PI / 180.0))); - m_rightVec = - new Vector2d( - Math.cos(rightMotorAngle * (Math.PI / 180.0)), - Math.sin(rightMotorAngle * (Math.PI / 180.0))); - m_backVec = - new Vector2d( - Math.cos(backMotorAngle * (Math.PI / 180.0)), - Math.sin(backMotorAngle * (Math.PI / 180.0))); - SendableRegistry.addChild(this, m_leftMotor); - SendableRegistry.addChild(this, m_rightMotor); - SendableRegistry.addChild(this, m_backMotor); - instances++; - SendableRegistry.addLW(this, "KilloughDrive", instances); - } - - @Override - public void close() { - SendableRegistry.remove(this); - } - - /** - * Drive method 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. - */ - public void driveCartesian(double ySpeed, double xSpeed, double zRotation) { - driveCartesian(ySpeed, xSpeed, zRotation, 0.0); - } - - /** - * Drive method 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. - */ - public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) { - if (!m_reported) { - HAL.report( - tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughCartesian, 3); - m_reported = true; - } - - ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband); - xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband); - - var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); - - m_leftMotor.set(speeds.left * m_maxOutput); - m_rightMotor.set(speeds.right * m_maxOutput); - m_backMotor.set(speeds.back * m_maxOutput); - - feed(); - } - - /** - * Drive method for Killough platform. - * - *

Angles are measured counter-clockwise from straight ahead. The speed at which the robot - * drives (translation) is independent from its angle or rotation rate. - * - * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive. - * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180]. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is - * positive. - */ - public void drivePolar(double magnitude, double angle, double zRotation) { - if (!m_reported) { - HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughPolar, 3); - m_reported = true; - } - - driveCartesian( - magnitude * Math.sin(angle * (Math.PI / 180.0)), - magnitude * Math.cos(angle * (Math.PI / 180.0)), - zRotation, - 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. - * @return Wheel speeds [-1.0..1.0]. - */ - public WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation) { - return driveCartesianIK(ySpeed, xSpeed, zRotation, 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. - * @return Wheel speeds [-1.0..1.0]. - */ - 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(); - m_rightMotor.stopMotor(); - m_backMotor.stopMotor(); - feed(); - } - - @Override - public String getDescription() { - return "KilloughDrive"; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("KilloughDrive"); - builder.setActuator(true); - builder.setSafeState(this::stopMotor); - builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set); - builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set); - builder.addDoubleProperty("Back Motor Speed", m_backMotor::get, m_backMotor::set); - } -} 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 4888f84e98..43b6754943 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 @@ -10,6 +10,8 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -36,9 +38,10 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController; *

Each drive() function provides different inverse kinematic relations for a Mecanum drive * robot. * - *

The positive Y axis points ahead, the positive X axis points right, and the positive Z axis - * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is - * positive. + *

This library uses the NWU axes convention (North-West-Up as external reference in the world + * frame). The positive X axis points ahead, the positive Y axis points to the left, and the + * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation + * around the Z axis is positive. * *

Note: the axis conventions used in this class differ from DifferentialDrive. This may change * in a future year's WPILib release. @@ -131,42 +134,42 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea /** * Drive method for Mecanum platform. * - *

Angles are measured clockwise from the positive X axis. The robot's speed is independent - * from its angle or rotation rate. + *

Angles are measured counterclockwise 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]. Forward is positive. - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. */ - public void driveCartesian(double ySpeed, double xSpeed, double zRotation) { - driveCartesian(ySpeed, xSpeed, zRotation, 0.0); + public void driveCartesian(double xSpeed, double ySpeed, double zRotation) { + driveCartesian(xSpeed, ySpeed, zRotation, new Rotation2d()); } /** * Drive method for Mecanum platform. * - *

Angles are measured clockwise from the positive X axis. The robot's speed is independent - * from its angle or rotation rate. + *

Angles are measured counterclockwise 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]. Forward is positive. - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * @param xSpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is positive. + * @param ySpeed The robot's speed along the X axis [-1.0..1.0]. Left is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. - * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this - * to implement field-oriented controls. + * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented + * controls. */ - public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) { + public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) { if (!m_reported) { HAL.report( tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumCartesian, 4); m_reported = true; } - ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband); xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband); + ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband); - var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle); + var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle); m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput); m_frontRightMotor.set(speeds.frontRight * m_maxOutput); @@ -179,41 +182,38 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea /** * Drive method for Mecanum platform. * - *

Angles are measured counter-clockwise from straight ahead. The speed at which the robot + *

Angles are measured counterclockwise from straight ahead. The speed at which the robot * drives (translation) is independent from its angle or rotation rate. * * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive. - * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180]. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * @param angle The gyro heading around the Z axis at which the robot drives. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. */ - public void drivePolar(double magnitude, double angle, double zRotation) { + public void drivePolar(double magnitude, Rotation2d angle, double zRotation) { if (!m_reported) { HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumPolar, 4); m_reported = true; } driveCartesian( - magnitude * Math.cos(angle * (Math.PI / 180.0)), - magnitude * Math.sin(angle * (Math.PI / 180.0)), - zRotation, - 0.0); + magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, new Rotation2d()); } /** * Cartesian inverse kinematics for Mecanum platform. * - *

Angles are measured clockwise from the positive X axis. The robot's speed is independent - * from its angle or rotation rate. + *

Angles are measured counterclockwise 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]. Forward is positive. - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. * @return Wheel speeds [-1.0..1.0]. */ - public static WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation) { - return driveCartesianIK(ySpeed, xSpeed, zRotation, 0.0); + public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) { + return driveCartesianIK(xSpeed, ySpeed, zRotation, new Rotation2d()); } /** @@ -222,28 +222,27 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea *

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]. Forward is positive. - * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive. - * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is + * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive. + * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive. + * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is * positive. - * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this - * to implement field-oriented controls. + * @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented + * controls. * @return Wheel speeds [-1.0..1.0]. */ public static WheelSpeeds driveCartesianIK( - double ySpeed, double xSpeed, double zRotation, double gyroAngle) { - ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); + double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) { xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0); + ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0); // Compensate for gyro angle. - Vector2d input = new Vector2d(ySpeed, xSpeed); - input.rotate(-gyroAngle); + var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus()); 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; + wheelSpeeds[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation; + wheelSpeeds[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation; + wheelSpeeds[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation; + wheelSpeeds[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation; normalize(wheelSpeeds); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java deleted file mode 100644 index 79f37333d8..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java +++ /dev/null @@ -1,63 +0,0 @@ -// 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; - -/** This is a 2D vector struct that supports basic vector operations. */ -@SuppressWarnings("MemberName") -public class Vector2d { - public double x; - public double y; - - public Vector2d() {} - - public Vector2d(double x, double y) { - this.x = x; - this.y = y; - } - - /** - * Rotate a vector in Cartesian space. - * - * @param angle angle in degrees by which to rotate vector counter-clockwise. - */ - public void rotate(double angle) { - double cosA = Math.cos(angle * (Math.PI / 180.0)); - double sinA = Math.sin(angle * (Math.PI / 180.0)); - double[] out = new double[2]; - out[0] = x * cosA - y * sinA; - out[1] = x * sinA + y * cosA; - x = out[0]; - y = out[1]; - } - - /** - * Returns dot product of this vector with argument. - * - * @param vec Vector with which to perform dot product. - * @return Dot product of this vector with argument. - */ - public double dot(Vector2d vec) { - return x * vec.x + y * vec.y; - } - - /** - * Returns magnitude of vector. - * - * @return Magnitude of vector. - */ - public double magnitude() { - return Math.sqrt(x * x + y * y); - } - - /** - * Returns scalar projection of this vector onto argument. - * - * @param vec Vector onto which to project this vector. - * @return scalar projection of this vector onto argument. - */ - public double scalarProject(Vector2d vec) { - return dot(vec) / vec.magnitude(); - } -} 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 index afeac0b93b..4675a7f2be 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java @@ -18,12 +18,12 @@ class DifferentialDriveTest { assertEquals(1.0, speeds.right, 1e-9); // Forward left turn - speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false); + speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false); assertEquals(0.0, speeds.left, 1e-9); assertEquals(0.5, speeds.right, 1e-9); // Forward right turn - speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false); + speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false); assertEquals(0.5, speeds.left, 1e-9); assertEquals(0.0, speeds.right, 1e-9); @@ -33,32 +33,32 @@ class DifferentialDriveTest { assertEquals(-1.0, speeds.right, 1e-9); // Backward left turn - speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false); + speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false); assertEquals(-0.5, speeds.left, 1e-9); assertEquals(0.0, speeds.right, 1e-9); // Backward right turn - speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false); + speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false); assertEquals(0.0, speeds.left, 1e-9); assertEquals(-0.5, speeds.right, 1e-9); // Left turn (xSpeed with negative sign) - speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false); assertEquals(-1.0, speeds.left, 1e-9); assertEquals(1.0, speeds.right, 1e-9); // Left turn (xSpeed with positive sign) - speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false); assertEquals(-1.0, speeds.left, 1e-9); assertEquals(1.0, speeds.right, 1e-9); // Right turn (xSpeed with negative sign) - speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false); assertEquals(1.0, speeds.left, 1e-9); assertEquals(-1.0, speeds.right, 1e-9); // Right turn (xSpeed with positive sign) - speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false); assertEquals(1.0, speeds.left, 1e-9); assertEquals(-1.0, speeds.right, 1e-9); } @@ -71,12 +71,12 @@ class DifferentialDriveTest { assertEquals(1.0, speeds.right, 1e-9); // Forward left turn - speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true); + speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true); assertEquals(0.0, speeds.left, 1e-9); assertEquals(0.25, speeds.right, 1e-9); // Forward right turn - speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true); + speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true); assertEquals(0.25, speeds.left, 1e-9); assertEquals(0.0, speeds.right, 1e-9); @@ -86,32 +86,32 @@ class DifferentialDriveTest { assertEquals(-1.0, speeds.right, 1e-9); // Backward left turn - speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true); + speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true); assertEquals(-0.25, speeds.left, 1e-9); assertEquals(0.0, speeds.right, 1e-9); // Backward right turn - speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true); + speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true); assertEquals(0.0, speeds.left, 1e-9); assertEquals(-0.25, speeds.right, 1e-9); // Left turn (xSpeed with negative sign) - speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false); assertEquals(-1.0, speeds.left, 1e-9); assertEquals(1.0, speeds.right, 1e-9); // Left turn (xSpeed with positive sign) - speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false); assertEquals(-1.0, speeds.left, 1e-9); assertEquals(1.0, speeds.right, 1e-9); // Right turn (xSpeed with negative sign) - speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false); assertEquals(1.0, speeds.left, 1e-9); assertEquals(-1.0, speeds.right, 1e-9); // Right turn (xSpeed with positive sign) - speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false); + speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false); assertEquals(1.0, speeds.left, 1e-9); assertEquals(-1.0, speeds.right, 1e-9); } @@ -124,12 +124,12 @@ class DifferentialDriveTest { assertEquals(1.0, speeds.right, 1e-9); // Forward left turn - speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false); + speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false); assertEquals(0.25, speeds.left, 1e-9); assertEquals(0.75, speeds.right, 1e-9); // Forward right turn - speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false); + speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false); assertEquals(0.75, speeds.left, 1e-9); assertEquals(0.25, speeds.right, 1e-9); @@ -139,12 +139,12 @@ class DifferentialDriveTest { assertEquals(-1.0, speeds.right, 1e-9); // Backward left turn - speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false); + speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false); assertEquals(-0.75, speeds.left, 1e-9); assertEquals(-0.25, speeds.right, 1e-9); // Backward right turn - speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false); + speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false); assertEquals(-0.25, speeds.left, 1e-9); assertEquals(-0.75, speeds.right, 1e-9); } @@ -157,12 +157,12 @@ class DifferentialDriveTest { assertEquals(1.0, speeds.right, 1e-9); // Forward left turn - speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true); + speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true); assertEquals(0.0, speeds.left, 1e-9); assertEquals(1.0, speeds.right, 1e-9); // Forward right turn - speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true); + speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true); assertEquals(1.0, speeds.left, 1e-9); assertEquals(0.0, speeds.right, 1e-9); @@ -172,12 +172,12 @@ class DifferentialDriveTest { assertEquals(-1.0, speeds.right, 1e-9); // Backward left turn - speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true); + speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true); assertEquals(-1.0, speeds.left, 1e-9); assertEquals(0.0, speeds.right, 1e-9); // Backward right turn - speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true); + speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true); assertEquals(0.0, speeds.left, 1e-9); assertEquals(-1.0, speeds.right, 1e-9); } @@ -261,12 +261,12 @@ class DifferentialDriveTest { assertEquals(1.0, right.get(), 1e-9); // Forward left turn - drive.arcadeDrive(0.5, -0.5, false); + 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); + drive.arcadeDrive(0.5, -0.5, false); assertEquals(0.5, left.get(), 1e-9); assertEquals(0.0, right.get(), 1e-9); @@ -276,12 +276,12 @@ class DifferentialDriveTest { assertEquals(-1.0, right.get(), 1e-9); // Backward left turn - drive.arcadeDrive(-0.5, -0.5, false); + 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); + drive.arcadeDrive(-0.5, -0.5, false); assertEquals(0.0, left.get(), 1e-9); assertEquals(-0.5, right.get(), 1e-9); } @@ -299,12 +299,12 @@ class DifferentialDriveTest { assertEquals(1.0, right.get(), 1e-9); // Forward left turn - drive.arcadeDrive(0.5, -0.5, true); + 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); + drive.arcadeDrive(0.5, -0.5, true); assertEquals(0.25, left.get(), 1e-9); assertEquals(0.0, right.get(), 1e-9); @@ -314,12 +314,12 @@ class DifferentialDriveTest { assertEquals(-1.0, right.get(), 1e-9); // Backward left turn - drive.arcadeDrive(-0.5, -0.5, true); + 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); + drive.arcadeDrive(-0.5, -0.5, true); assertEquals(0.0, left.get(), 1e-9); assertEquals(-0.25, right.get(), 1e-9); } @@ -337,12 +337,12 @@ class DifferentialDriveTest { assertEquals(1.0, right.get(), 1e-9); // Forward left turn - drive.curvatureDrive(0.5, -0.5, false); + 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); + drive.curvatureDrive(0.5, -0.5, false); assertEquals(0.75, left.get(), 1e-9); assertEquals(0.25, right.get(), 1e-9); @@ -352,12 +352,12 @@ class DifferentialDriveTest { assertEquals(-1.0, right.get(), 1e-9); // Backward left turn - drive.curvatureDrive(-0.5, -0.5, false); + 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); + drive.curvatureDrive(-0.5, -0.5, false); assertEquals(-0.25, left.get(), 1e-9); assertEquals(-0.75, right.get(), 1e-9); } @@ -375,12 +375,12 @@ class DifferentialDriveTest { assertEquals(1.0, right.get(), 1e-9); // Forward left turn - drive.curvatureDrive(0.5, -0.5, true); + 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); + drive.curvatureDrive(0.5, -0.5, true); assertEquals(1.0, left.get(), 1e-9); assertEquals(0.0, right.get(), 1e-9); @@ -390,12 +390,12 @@ class DifferentialDriveTest { assertEquals(-1.0, right.get(), 1e-9); // Backward left turn - drive.curvatureDrive(-0.5, -0.5, true); + 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); + drive.curvatureDrive(-0.5, -0.5, true); assertEquals(0.0, left.get(), 1e-9); assertEquals(-1.0, 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 deleted file mode 100644 index 99858d94de..0000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java +++ /dev/null @@ -1,205 +0,0 @@ -// 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 testCartesianIK() { - var left = new MockMotorController(); - var right = new MockMotorController(); - var back = new MockMotorController(); - var drive = new KilloughDrive(left, right, back); - - // Forward - var speeds = drive.driveCartesianIK(1.0, 0.0, 0.0); - assertEquals(0.5, speeds.left, 1e-9); - assertEquals(-0.5, speeds.right, 1e-9); - assertEquals(0.0, speeds.back, 1e-9); - - // Left - speeds = drive.driveCartesianIK(0.0, -1.0, 0.0); - assertEquals(-Math.sqrt(3) / 2, speeds.left, 1e-9); - assertEquals(-Math.sqrt(3) / 2, speeds.right, 1e-9); - assertEquals(1.0, speeds.back, 1e-9); - - // Right - speeds = drive.driveCartesianIK(0.0, 1.0, 0.0); - assertEquals(Math.sqrt(3) / 2, speeds.left, 1e-9); - assertEquals(Math.sqrt(3) / 2, speeds.right, 1e-9); - assertEquals(-1.0, speeds.back, 1e-9); - - // Rotate CCW - speeds = drive.driveCartesianIK(0.0, 0.0, -1.0); - assertEquals(-1.0, speeds.left, 1e-9); - assertEquals(-1.0, speeds.right, 1e-9); - assertEquals(-1.0, speeds.back, 1e-9); - - // Rotate CW - speeds = drive.driveCartesianIK(0.0, 0.0, 1.0); - assertEquals(1.0, speeds.left, 1e-9); - assertEquals(1.0, speeds.right, 1e-9); - assertEquals(1.0, speeds.back, 1e-9); - } - - @Test - void testCartesianIKGyro90CW() { - var left = new MockMotorController(); - var right = new MockMotorController(); - var back = new MockMotorController(); - var drive = new KilloughDrive(left, right, back); - - // Forward in global frame; left in robot frame - var speeds = drive.driveCartesianIK(1.0, 0.0, 0.0, 90.0); - assertEquals(-Math.sqrt(3) / 2, speeds.left, 1e-9); - assertEquals(-Math.sqrt(3) / 2, speeds.right, 1e-9); - assertEquals(1.0, speeds.back, 1e-9); - - // Left in global frame; backward in robot frame - speeds = drive.driveCartesianIK(0.0, -1.0, 0.0, 90.0); - assertEquals(-0.5, speeds.left, 1e-9); - assertEquals(0.5, speeds.right, 1e-9); - assertEquals(0.0, speeds.back, 1e-9); - - // Right in global frame; forward in robot frame - speeds = drive.driveCartesianIK(0.0, 1.0, 0.0, 90.0); - assertEquals(0.5, speeds.left, 1e-9); - assertEquals(-0.5, speeds.right, 1e-9); - assertEquals(0.0, speeds.back, 1e-9); - - // Rotate CCW - speeds = drive.driveCartesianIK(0.0, 0.0, -1.0, 90.0); - assertEquals(-1.0, speeds.left, 1e-9); - assertEquals(-1.0, speeds.right, 1e-9); - assertEquals(-1.0, speeds.back, 1e-9); - - // Rotate CW - speeds = drive.driveCartesianIK(0.0, 0.0, 1.0, 90.0); - assertEquals(1.0, speeds.left, 1e-9); - assertEquals(1.0, speeds.right, 1e-9); - assertEquals(1.0, speeds.back, 1e-9); - } - - @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 index bf3349c6d8..034ba10c0c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java @@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj.drive; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.motorcontrol.MockMotorController; import org.junit.jupiter.api.Test; @@ -51,35 +52,35 @@ class MecanumDriveTest { @Test void testCartesianIKGyro90CW() { // Forward in global frame; left in robot frame - var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, 90.0); + var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.0)); assertEquals(-1.0, speeds.frontLeft, 1e-9); assertEquals(1.0, speeds.frontRight, 1e-9); assertEquals(1.0, speeds.rearLeft, 1e-9); assertEquals(-1.0, speeds.rearRight, 1e-9); // Left in global frame; backward in robot frame - speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, 90.0); + speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.0)); assertEquals(-1.0, speeds.frontLeft, 1e-9); assertEquals(-1.0, speeds.frontRight, 1e-9); assertEquals(-1.0, speeds.rearLeft, 1e-9); assertEquals(-1.0, speeds.rearRight, 1e-9); // Right in global frame; forward in robot frame - speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, 90.0); + speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.0)); assertEquals(1.0, speeds.frontLeft, 1e-9); assertEquals(1.0, speeds.frontRight, 1e-9); assertEquals(1.0, speeds.rearLeft, 1e-9); assertEquals(1.0, speeds.rearRight, 1e-9); // Rotate CCW - speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, 90.0); + speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.0)); assertEquals(-1.0, speeds.frontLeft, 1e-9); assertEquals(1.0, speeds.frontRight, 1e-9); assertEquals(-1.0, speeds.rearLeft, 1e-9); assertEquals(1.0, speeds.rearRight, 1e-9); // Rotate CW - speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, 90.0); + speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0)); assertEquals(1.0, speeds.frontLeft, 1e-9); assertEquals(-1.0, speeds.frontRight, 1e-9); assertEquals(1.0, speeds.rearLeft, 1e-9); @@ -141,35 +142,35 @@ class MecanumDriveTest { drive.setDeadband(0.0); // Forward in global frame; left in robot frame - drive.driveCartesian(1.0, 0.0, 0.0, 90.0); + drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.fromDegrees(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); + drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.fromDegrees(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); + drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.fromDegrees(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); + drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.fromDegrees(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); + drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0)); assertEquals(1.0, fl.get(), 1e-9); assertEquals(-1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); @@ -186,35 +187,35 @@ class MecanumDriveTest { drive.setDeadband(0.0); // Forward - drive.drivePolar(1.0, 0.0, 0.0); + drive.drivePolar(1.0, Rotation2d.fromDegrees(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); + drive.drivePolar(1.0, Rotation2d.fromDegrees(-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); + drive.drivePolar(1.0, Rotation2d.fromDegrees(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); + drive.drivePolar(0.0, Rotation2d.fromDegrees(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); + drive.drivePolar(0.0, Rotation2d.fromDegrees(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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java index 8b4391c25a..042eda9bcf 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java @@ -51,6 +51,6 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { m_robotDrive.driveCartesian( - -m_joystick.getY(), m_joystick.getX(), m_joystick.getZ(), m_gyro.getAngle()); + -m_joystick.getY(), m_joystick.getX(), m_joystick.getZ(), m_gyro.getRotation2d()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index b566f187c2..1b77080572 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -113,7 +113,7 @@ public class DriveSubsystem extends SubsystemBase { */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { if (fieldRelative) { - m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle()); + m_drive.driveCartesian(ySpeed, xSpeed, rot, m_gyro.getRotation2d()); } else { m_drive.driveCartesian(ySpeed, xSpeed, rot); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java index 1029f0f3fe..a19cc81e2a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java @@ -40,8 +40,8 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { - // Use the joystick X axis for lateral movement, Y axis for forward + // Use the joystick X axis for forward movement, Y axis for lateral // movement, and Z axis for rotation. - m_robotDrive.driveCartesian(-m_stick.getY(), m_stick.getX(), m_stick.getZ(), 0.0); + m_robotDrive.driveCartesian(-m_stick.getY(), m_stick.getX(), m_stick.getZ()); } }