mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +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
@@ -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.
|
||||
|
||||
@@ -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 <memory>
|
||||
#include <string>
|
||||
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
|
||||
#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:
|
||||
* <pre>
|
||||
* /_____\
|
||||
* / \ / \
|
||||
* \ /
|
||||
* ---
|
||||
* </pre>
|
||||
*
|
||||
* 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<KilloughDrive> {
|
||||
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
|
||||
@@ -7,10 +7,12 @@
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
|
||||
#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;
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user