Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] DifferentialDrive: Remove right side inversion (#3340)

Browse Source 
Also refactor drive inverse kinematics into separate functions.
This allows composing them with operations separate from the drive
class.
This commit is contained in:
Tyler Veness
2021-05-21 22:34:16 -07:00
committed by GitHub
parent 8dd8d4d2d4
commit 0768c39036
17 changed files with 1533 additions and 478 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
View File

@@ -95,23 +95,15 @@ class SpeedController;
* Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
* so that the full range is still used. This deadband value can be changed
* with SetDeadband().
*
* <p>RobotDrive porting guide:
* <br>TankDrive(double, double, bool) is equivalent to
* RobotDrive's TankDrive(double, double, bool) if a deadband of 0 is used.
* <br>ArcadeDrive(double, double, bool) is equivalent to
* RobotDrive's ArcadeDrive(double, double, bool) if a deadband of 0 is used
* and the the rotation input is inverted eg ArcadeDrive(y, -rotation, false)
* <br>CurvatureDrive(double, double, bool) is similar in concept to
* RobotDrive's Drive(double, double) with the addition of a quick turn
* mode. However, it is not designed to give exactly the same response.
*/
class DifferentialDrive : public RobotDriveBase,
public Sendable,
public SendableHelper<DifferentialDrive> {
public:
static constexpr double kDefaultQuickStopThreshold = 0.2;
static constexpr double kDefaultQuickStopAlpha = 0.1;
struct WheelSpeeds {
double left = 0.0;
double right = 0.0;
};
/**
* Construct a DifferentialDrive.
@@ -148,14 +140,14 @@ class DifferentialDrive : public RobotDriveBase,
* high speeds. Also handles the robot's quick turn functionality - "quick
* turn" overrides constant-curvature turning for turn-in-place maneuvers.
*
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward
* is positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param isQuickTurn If set, overrides constant-curvature turning for
* turn-in-place maneuvers.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0].
* Forward is positive.
* @param zRotation The robot's rotation rate around the Z axis
* [-1.0..1.0]. Clockwise is positive.
* @param allowTurnInPlace If set, overrides constant-curvature turning for
* turn-in-place maneuvers.
*/
void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn);
void CurvatureDrive(double xSpeed, double zRotation, bool allowTurnInPlace);
/**
* Tank drive method for differential drive platform.
@@ -169,51 +161,49 @@ class DifferentialDrive : public RobotDriveBase,
void TankDrive(double leftSpeed, double rightSpeed, bool squareInputs = true);
/**
* Sets the QuickStop speed threshold in curvature drive.
* Arcade drive inverse kinematics for differential drive platform.
*
* QuickStop compensates for the robot's moment of inertia when stopping after
* a QuickTurn.
* Note: Some drivers may prefer inverted rotation controls. This can be done
* by negating the value passed for rotation.
*
* While QuickTurn is enabled, the QuickStop accumulator takes on the rotation
* rate value outputted by the low-pass filter when the robot's speed along
* the X axis is below the threshold. When QuickTurn is disabled, the
* accumulator's value is applied against the computed angular power request
* to slow the robot's rotation.
*
* @param threshold X speed below which quick stop accumulator will receive
* rotation rate values [0..1.0].
* @param xSpeed The speed at which the robot should drive along the X
* axis [-1.0..1.0]. Forward is positive.
* @param zRotation The rotation rate of the robot around the Z axis
* [-1.0..1.0]. Clockwise is positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
*/
void SetQuickStopThreshold(double threshold);
static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation,
bool squareInputs = true);
/**
* Sets the low-pass filter gain for QuickStop in curvature drive.
* Curvature drive inverse kinematics for differential drive platform.
*
* The low-pass filter filters incoming rotation rate commands to smooth out
* high frequency changes.
* The rotation argument controls the curvature of the robot's path rather
* than its rate of heading change. This makes the robot more controllable at
* high speeds. Also handles the robot's quick turn functionality - "quick
* turn" overrides constant-curvature turning for turn-in-place maneuvers.
*
* @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in
* slower output changes. Values between 1.0 and 2.0 result in
* output oscillation. Values below 0.0 and above 2.0 are
* unstable.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0].
* Forward is positive.
* @param zRotation The robot's rotation rate around the Z axis
* [-1.0..1.0]. Clockwise is positive.
* @param allowTurnInPlace If set, overrides constant-curvature turning for
* turn-in-place maneuvers.
*/
void SetQuickStopAlpha(double alpha);
static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation,
bool allowTurnInPlace);
/**
* Gets if the power sent to the right side of the drivetrain is multiplied by
* -1.
* Tank drive inverse kinematics for differential drive platform.
*
* @return true if the right side is inverted
* @param leftSpeed The robot left side's speed along the X axis
* [-1.0..1.0]. Forward is positive.
* @param rightSpeed The robot right side's speed along the X axis
* [-1.0..1.0]. Forward is positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
*/
bool IsRightSideInverted() const;
/**
* Sets if the power sent to the right side of the drivetrain should be
* multiplied by -1.
*
* @param rightSideInverted true if right side power should be multiplied by
* -1
*/
void SetRightSideInverted(bool rightSideInverted);
static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed,
bool squareInputs = true);
void StopMotor() override;
void GetDescription(wpi::raw_ostream& desc) const override;
@@ -223,11 +213,6 @@ class DifferentialDrive : public RobotDriveBase,
private:
SpeedController* m_leftMotor;
SpeedController* m_rightMotor;
double m_quickStopThreshold = kDefaultQuickStopThreshold;
double m_quickStopAlpha = kDefaultQuickStopAlpha;
double m_quickStopAccumulator = 0.0;
double m_rightSideInvertMultiplier = -1.0;
};
#if defined(_MSC_VER)

24
wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
View File

@@ -62,6 +62,12 @@ class KilloughDrive : public RobotDriveBase,
static constexpr double kDefaultRightMotorAngle = 120.0;
static constexpr double kDefaultBackMotorAngle = 270.0;
struct WheelSpeeds {
double left = 0.0;
double right = 0.0;
double back = 0.0;
};
/**
* Construct a Killough drive with the given motors and default motor angles.
*
@@ -134,6 +140,24 @@ class KilloughDrive : public RobotDriveBase,
*/
void DrivePolar(double magnitude, double angle, double zRotation);
/**
* Cartesian inverse kinematics for Killough platform.
*
* Angles are measured clockwise from the positive X axis. The robot's speed
* is independent from its angle or rotation rate.
*
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is
* positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param gyroAngle The current angle reading from the gyro in degrees around
* the Z axis. Use this to implement field-oriented controls.
*/
WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation,
double gyroAngle = 0.0);
void StopMotor() override;
void GetDescription(wpi::raw_ostream& desc) const override;

34
wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
View File

@@ -76,6 +76,13 @@ class MecanumDrive : public RobotDriveBase,
public Sendable,
public SendableHelper<MecanumDrive> {
public:
struct WheelSpeeds {
double frontLeft = 0.0;
double frontRight = 0.0;
double rearLeft = 0.0;
double rearRight = 0.0;
};
/**
* Construct a MecanumDrive.
*
@@ -124,21 +131,22 @@ class MecanumDrive : public RobotDriveBase,
void DrivePolar(double magnitude, double angle, double zRotation);
/**
* Gets if the power sent to the right side of the drivetrain is multiplied by
* -1.
* Cartesian inverse kinematics for Mecanum platform.
*
* @return true if the right side is inverted
*/
bool IsRightSideInverted() const;
/**
* Sets if the power sent to the right side of the drivetrain should be
* multiplied by -1.
* Angles are measured clockwise from the positive X axis. The robot's speed
* is independent from its angle or rotation rate.
*
* @param rightSideInverted true if right side power should be multiplied by
* -1
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is
* positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param gyroAngle The current angle reading from the gyro in degrees around
* the Z axis. Use this to implement field-oriented controls.
*/
void SetRightSideInverted(bool rightSideInverted);
static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed,
double zRotation, double gyroAngle = 0.0);
void StopMotor() override;
void GetDescription(wpi::raw_ostream& desc) const override;
@@ -151,8 +159,6 @@ class MecanumDrive : public RobotDriveBase,
SpeedController* m_frontRightMotor;
SpeedController* m_rearRightMotor;
double m_rightSideInvertMultiplier = -1.0;
bool reported = false;
};

4
wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
View File

@@ -76,13 +76,13 @@ class RobotDriveBase : public MotorSafety {
* @param value value to clip
* @param deadband range around zero
*/
double ApplyDeadband(double number, double deadband);
static double ApplyDeadband(double number, double deadband);
/**
* Normalize all wheel speeds if the magnitude of any wheel is greater than
* 1.0.
*/
void Normalize(wpi::MutableArrayRef<double> wheelSpeeds);
static void Normalize(wpi::MutableArrayRef<double> wheelSpeeds);
double m_deadband = 0.02;
double m_maxOutput = 1.0;