Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Improved Drive docs and fix implementation bugs (#774)

Browse Source 
I also found some inconsistencies in MecanumDrive and KilloughDrive and fixed
them.

Drive now uses the NED axes convention. Therefore, the positive X axis points
ahead, the positive Y axis points to the right, and the positive Z axis points
down.

Translation in X assumes forward is positive. Translation in Y assumes right is
positive. Rotation rate assumes clockwise is positive. Angles are measured
clockwise from the positive X axis.

Based on the angle origin convention, DrivePolar() for both Mecanum and Killough
needed the normalization removed, sine used to compute the Y component, and
cosine used to compute the X component.

The vector rotation done in DriveCartesian() needs to rotate by a negative angle
instead of positive to undo the robot's rotation. RobotDrive assumed a clockwise
angle and sensors returned counter-clockwise angles, which is why it used a
positive angle for rotation.
This commit is contained in:
Tyler Veness
2017-11-26 18:36:51 -08:00
committed by Peter Johnson
parent 7a250a1b93
commit 34c44b7ae9
9 changed files with 409 additions and 254 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpilibc/src/main/native/include/Drive/DifferentialDrive.h
View File

@@ -73,18 +73,33 @@ class SpeedController;
* Each Drive() function provides different inverse kinematic relations for a
* differential drive robot. Motor outputs for the right side are negated, so
* motor direction inversion by the user is usually unnecessary.
*
* 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.
*/
class DifferentialDrive : public RobotDriveBase {
public:
static constexpr double kDefaultQuickStopThreshold = 0.2;
static constexpr double kDefaultQuickStopAlpha = 0.1;
DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor);
virtual ~DifferentialDrive() = default;
DifferentialDrive(const DifferentialDrive&) = delete;
DifferentialDrive& operator=(const DifferentialDrive&) = delete;
void ArcadeDrive(double y, double rotation, bool squaredInputs = true);
void CurvatureDrive(double y, double rotation, bool isQuickTurn);
void TankDrive(double left, double right, bool squaredInputs = true);
void ArcadeDrive(double xSpeed, double zRotation, bool squaredInputs = true);
void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn);
void TankDrive(double leftSpeed, double rightSpeed,
bool squaredInputs = true);
void SetQuickStopThreshold(double threshold);
void SetQuickStopAlpha(double alpha);
void StopMotor() override;
void GetDescription(llvm::raw_ostream& desc) const override;
@@ -93,6 +108,8 @@ class DifferentialDrive : public RobotDriveBase {
SpeedController& m_leftMotor;
SpeedController& m_rightMotor;
double m_quickStopThreshold = kDefaultQuickStopThreshold;
double m_quickStopAlpha = kDefaultQuickStopAlpha;
double m_quickStopAccumulator = 0.0;
};

16
wpilibc/src/main/native/include/Drive/KilloughDrive.h
View File

@@ -35,9 +35,21 @@ class SpeedController;
* 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.
*/
class KilloughDrive : public RobotDriveBase {
public:
static constexpr double kDefaultLeftMotorAngle = 60.0;
static constexpr double kDefaultRightMotorAngle = 120.0;
static constexpr double kDefaultBackMotorAngle = 270.0;
KilloughDrive(SpeedController& leftMotor, SpeedController& rightMotor,
SpeedController& backMotor);
KilloughDrive(SpeedController& leftMotor, SpeedController& rightMotor,
@@ -48,9 +60,9 @@ class KilloughDrive : public RobotDriveBase {
KilloughDrive(const KilloughDrive&) = delete;
KilloughDrive& operator=(const KilloughDrive&) = delete;
void DriveCartesian(double x, double y, double rotation,
void DriveCartesian(double ySpeed, double xSpeed, double zRotation,
double gyroAngle = 0.0);
void DrivePolar(double magnitude, double angle, double rotation);
void DrivePolar(double magnitude, double angle, double zRotation);
void StopMotor() override;
void GetDescription(llvm::raw_ostream& desc) const override;

8
wpilibc/src/main/native/include/Drive/MecanumDrive.h
View File

@@ -36,6 +36,14 @@ class SpeedController;
* Each Drive() function provides different inverse kinematic relations for a
* Mecanum drive robot. Motor outputs for the right side are negated, so motor
* direction inversion by the user is usually unnecessary.
*
* 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.
*/
class MecanumDrive : public RobotDriveBase {
public: