Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

69
wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp
View File

@@ -36,38 +36,41 @@ MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
/**
* Drive method for Mecanum platform.
*
* @param x The speed that the robot should drive in the X direction.
* [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely
* independent of the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to
* implement field-oriented controls.
* 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 MecanumDrive::DriveCartesian(double x, double y, double rotation,
double gyroAngle) {
void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
double zRotation, double gyroAngle) {
if (!reported) {
HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
HALUsageReporting::kRobotDrive_MecanumCartesian);
reported = true;
}
x = Limit(x);
x = ApplyDeadband(x, m_deadband);
ySpeed = Limit(ySpeed);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
y = Limit(y);
y = ApplyDeadband(y, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
Vector2d input{x, y};
input.Rotate(gyroAngle);
Vector2d input{ySpeed, xSpeed};
input.Rotate(-gyroAngle);
double wheelSpeeds[4];
wheelSpeeds[kFrontLeft] = input.x + input.y + rotation;
wheelSpeeds[kFrontRight] = input.x - input.y + rotation;
wheelSpeeds[kRearLeft] = -input.x + input.y + rotation;
wheelSpeeds[kRearRight] = -input.x - input.y + rotation;
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;
Normalize(wheelSpeeds);
@@ -82,26 +85,26 @@ void MecanumDrive::DriveCartesian(double x, double y, double rotation,
/**
* Drive method for Mecanum platform.
*
* @param magnitude The speed that the robot should drive in a given direction.
* [-1.0..1.0]
* @param angle The direction the robot should drive in degrees. 0.0 is
* straight ahead. The direction and maginitude are independent
* of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely
* independent of the magnitude or direction. [-1.0..1.0]
* 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 MecanumDrive::DrivePolar(double magnitude, double angle, double rotation) {
void MecanumDrive::DrivePolar(double magnitude, double angle,
double zRotation) {
if (!reported) {
HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
HALUsageReporting::kRobotDrive_MecanumPolar);
reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * std::sqrt(2.0);
DriveCartesian(magnitude * std::cos(angle * (kPi / 180.0)),
magnitude * std::sin(angle * (kPi / 180.0)), rotation, 0.0);
DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
}
void MecanumDrive::StopMotor() {