mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpilib] Make drive classes follow NWU axes convention (#4079)
All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
This commit is contained in:
Tyler Veness
@@ -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;
|
||||
|
||||
@@ -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 <algorithm>
|
||||
#include <cmath>
|
||||
#include <numbers>
|
||||
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
#include <wpi/sendable/SendableRegistry.h>
|
||||
|
||||
#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); });
|
||||
}
|
||||
@@ -5,15 +5,13 @@
|
||||
#include "frc/drive/MecanumDrive.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <numbers>
|
||||
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
#include <wpi/sendable/SendableRegistry.h>
|
||||
|
||||
#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);
|
||||
|
||||
|
||||
@@ -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 <cmath>
|
||||
#include <numbers>
|
||||
|
||||
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();
|
||||
}
|
||||
Reference in New Issue
Block a user