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

75
wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp
View File

@@ -21,9 +21,8 @@ constexpr double kPi = 3.14159265358979323846;
/**
* Construct a Killough drive with the given motors and default motor angles.
*
* The default motor angles are 120, 60, and 270 degrees for the left, right,
* and back motors respectively, which make the wheels on each corner parallel
* to their respective opposite sides.
* 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.
*
@@ -34,13 +33,13 @@ constexpr double kPi = 3.14159265358979323846;
KilloughDrive::KilloughDrive(SpeedController& leftMotor,
SpeedController& rightMotor,
SpeedController& backMotor)
: KilloughDrive(leftMotor, rightMotor, backMotor, 120.0, 60.0, 270.0) {}
: KilloughDrive(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle,
kDefaultRightMotorAngle, kDefaultBackMotorAngle) {}
/**
* Construct a Killough drive with the given motors.
*
* Angles are measured in counter-clockwise degrees where zero degrees is
* straight ahead.
* 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.
@@ -68,37 +67,40 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor,
/**
* Drive method for Killough 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 KilloughDrive::DriveCartesian(double x, double y, double rotation,
double gyroAngle) {
void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
double zRotation, double gyroAngle) {
if (!reported) {
// HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
// HALUsageReporting::kRobotDrive_KilloughCartesian);
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[3];
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + rotation;
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + rotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + rotation;
wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
Normalize(wheelSpeeds);
@@ -112,13 +114,15 @@ void KilloughDrive::DriveCartesian(double x, double y, double rotation,
/**
* Drive method for Killough 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 KilloughDrive::DrivePolar(double magnitude, double angle,
double rotation) {
@@ -128,11 +132,8 @@ void KilloughDrive::DrivePolar(double magnitude, double angle,
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)), rotation, 0.0);
}
void KilloughDrive::StopMotor() {