mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +00:00
Improved Drive docs and fix implementation bugs (#774)
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
committed by
Peter Johnson
Peter Johnson
parent
7a250a1b93
commit
34c44b7ae9
@@ -66,11 +66,23 @@ import edu.wpi.first.wpilibj.hal.HAL;
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
|
||||
* points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
|
||||
* positive.
|
||||
*/
|
||||
public class DifferentialDrive extends RobotDriveBase {
|
||||
public static final double kDefaultQuickStopThreshold = 0.2;
|
||||
public static final double kDefaultQuickStopAlpha = 0.1;
|
||||
|
||||
private SpeedController m_leftMotor;
|
||||
private SpeedController m_rightMotor;
|
||||
|
||||
private double m_quickStopThreshold = kDefaultQuickStopThreshold;
|
||||
private double m_quickStopAlpha = kDefaultQuickStopAlpha;
|
||||
private double m_quickStopAccumulator = 0.0;
|
||||
private boolean m_reported = false;
|
||||
|
||||
@@ -88,63 +100,65 @@ public class DifferentialDrive extends RobotDriveBase {
|
||||
/**
|
||||
* Arcade drive method for differential drive platform.
|
||||
*
|
||||
* @param y The value to use for forwards/backwards. [-1.0..1.0]
|
||||
* @param rotation The value to use for the rotation right/left. [-1.0..1.0]
|
||||
* @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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void arcadeDrive(double y, double rotation) {
|
||||
arcadeDrive(y, rotation, true);
|
||||
public void arcadeDrive(double xSpeed, double zRotation) {
|
||||
arcadeDrive(xSpeed, zRotation, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive method for differential drive platform.
|
||||
*
|
||||
* @param y The value to use for forwards/backwards. [-1.0..1.0]
|
||||
* @param rotation The value to use for the rotation right/left [-1.0..1.0]
|
||||
* @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 squaredInputs If set, decreases the input sensitivity at low speeds.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void arcadeDrive(double y, double rotation, boolean squaredInputs) {
|
||||
public void arcadeDrive(double xSpeed, double zRotation, boolean squaredInputs) {
|
||||
if (!m_reported) {
|
||||
HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_ArcadeStandard);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
y = limit(y);
|
||||
y = applyDeadband(y, m_deadband);
|
||||
xSpeed = limit(xSpeed);
|
||||
xSpeed = applyDeadband(xSpeed, m_deadband);
|
||||
|
||||
rotation = limit(rotation);
|
||||
rotation = applyDeadband(rotation, m_deadband);
|
||||
zRotation = limit(zRotation);
|
||||
zRotation = applyDeadband(zRotation, m_deadband);
|
||||
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
// Square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power.
|
||||
if (squaredInputs) {
|
||||
y = Math.copySign(y * y, y);
|
||||
rotation = Math.copySign(rotation * rotation, rotation);
|
||||
xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
|
||||
zRotation = Math.copySign(zRotation * zRotation, zRotation);
|
||||
}
|
||||
|
||||
double leftMotorOutput;
|
||||
double rightMotorOutput;
|
||||
|
||||
double maxInput = Math.copySign(Math.max(Math.abs(y), Math.abs(rotation)), y);
|
||||
double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
|
||||
|
||||
if (y > 0.0) {
|
||||
if (xSpeed >= 0.0) {
|
||||
// First quadrant, else second quadrant
|
||||
if (rotation > 0.0) {
|
||||
if (zRotation >= 0.0) {
|
||||
leftMotorOutput = maxInput;
|
||||
rightMotorOutput = y - rotation;
|
||||
rightMotorOutput = xSpeed - zRotation;
|
||||
} else {
|
||||
leftMotorOutput = y + rotation;
|
||||
leftMotorOutput = xSpeed + zRotation;
|
||||
rightMotorOutput = maxInput;
|
||||
}
|
||||
} else {
|
||||
// Third quadrant, else fourth quadrant
|
||||
if (rotation > 0.0) {
|
||||
leftMotorOutput = y + rotation;
|
||||
if (zRotation >= 0.0) {
|
||||
leftMotorOutput = xSpeed + zRotation;
|
||||
rightMotorOutput = maxInput;
|
||||
} else {
|
||||
leftMotorOutput = maxInput;
|
||||
rightMotorOutput = y - rotation;
|
||||
rightMotorOutput = xSpeed - zRotation;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -162,38 +176,38 @@ public class DifferentialDrive extends RobotDriveBase {
|
||||
* robot's quick turn functionality - "quick turn" overrides constant-curvature turning for
|
||||
* turn-in-place maneuvers.
|
||||
*
|
||||
* @param y The value to use for forwards/backwards. [-1.0..1.0]
|
||||
* @param rotation The value to use for the rotation right/left. [-1.0..1.0]
|
||||
* @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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void curvatureDrive(double y, double rotation, boolean isQuickTurn) {
|
||||
public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) {
|
||||
if (!m_reported) {
|
||||
// HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_Curvature);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
y = limit(y);
|
||||
y = applyDeadband(y, m_deadband);
|
||||
xSpeed = limit(xSpeed);
|
||||
xSpeed = applyDeadband(xSpeed, m_deadband);
|
||||
|
||||
rotation = limit(rotation);
|
||||
rotation = applyDeadband(rotation, m_deadband);
|
||||
zRotation = limit(zRotation);
|
||||
zRotation = applyDeadband(zRotation, m_deadband);
|
||||
|
||||
double angularPower;
|
||||
boolean overPower;
|
||||
|
||||
if (isQuickTurn) {
|
||||
if (Math.abs(y) < 0.2) {
|
||||
final double alpha = 0.1;
|
||||
m_quickStopAccumulator =
|
||||
(1 - alpha) * m_quickStopAccumulator + alpha * limit(rotation) * 2;
|
||||
if (Math.abs(xSpeed) < m_quickStopThreshold) {
|
||||
m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator
|
||||
+ m_quickStopAlpha * limit(zRotation) * 2;
|
||||
}
|
||||
overPower = true;
|
||||
angularPower = rotation;
|
||||
angularPower = zRotation;
|
||||
} else {
|
||||
overPower = false;
|
||||
angularPower = Math.abs(y) * rotation - m_quickStopAccumulator;
|
||||
angularPower = Math.abs(xSpeed) * zRotation - m_quickStopAccumulator;
|
||||
|
||||
if (m_quickStopAccumulator > 1) {
|
||||
m_quickStopAccumulator -= 1;
|
||||
@@ -204,8 +218,8 @@ public class DifferentialDrive extends RobotDriveBase {
|
||||
}
|
||||
}
|
||||
|
||||
double leftMotorOutput = y + angularPower;
|
||||
double rightMotorOutput = y - angularPower;
|
||||
double leftMotorOutput = xSpeed + angularPower;
|
||||
double rightMotorOutput = xSpeed - angularPower;
|
||||
|
||||
// If rotation is overpowered, reduce both outputs to within acceptable range
|
||||
if (overPower) {
|
||||
@@ -233,45 +247,80 @@ public class DifferentialDrive extends RobotDriveBase {
|
||||
/**
|
||||
* Tank drive method for differential drive platform.
|
||||
*
|
||||
* @param left The value to use for left side motors. [-1.0..1.0]
|
||||
* @param right The value to use for right side motors. [-1.0..1.0]
|
||||
* @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
* @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
|
||||
* positive.
|
||||
*/
|
||||
public void tankDrive(double left, double right) {
|
||||
tankDrive(left, right, true);
|
||||
public void tankDrive(double leftSpeed, double rightSpeed) {
|
||||
tankDrive(leftSpeed, rightSpeed, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Tank drive method for differential drive platform.
|
||||
*
|
||||
* @param left The value to use for left side motors. [-1.0..1.0]
|
||||
* @param right The value to use for right side motors. [-1.0..1.0]
|
||||
* @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 squaredInputs If set, decreases the input sensitivity at low speeds.
|
||||
*/
|
||||
public void tankDrive(double left, double right, boolean squaredInputs) {
|
||||
public void tankDrive(double leftSpeed, double rightSpeed, boolean squaredInputs) {
|
||||
if (!m_reported) {
|
||||
HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_Tank);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
left = limit(left);
|
||||
left = applyDeadband(left, m_deadband);
|
||||
leftSpeed = limit(leftSpeed);
|
||||
leftSpeed = applyDeadband(leftSpeed, m_deadband);
|
||||
|
||||
right = limit(right);
|
||||
right = applyDeadband(right, m_deadband);
|
||||
rightSpeed = limit(rightSpeed);
|
||||
rightSpeed = applyDeadband(rightSpeed, m_deadband);
|
||||
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
// Square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power.
|
||||
if (squaredInputs) {
|
||||
left = Math.copySign(left * left, left);
|
||||
right = Math.copySign(right * right, right);
|
||||
leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
|
||||
rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
|
||||
}
|
||||
|
||||
m_leftMotor.set(left * m_maxOutput);
|
||||
m_rightMotor.set(-right * m_maxOutput);
|
||||
m_leftMotor.set(leftSpeed * m_maxOutput);
|
||||
m_rightMotor.set(-rightSpeed * m_maxOutput);
|
||||
|
||||
m_safetyHelper.feed();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the QuickStop speed threshold in curvature drive.
|
||||
*
|
||||
* <p>QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn.
|
||||
*
|
||||
* <p>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].
|
||||
*/
|
||||
public void setQuickStopThreshold(double threshold) {
|
||||
m_quickStopThreshold = threshold;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the low-pass filter gain for QuickStop in curvature drive.
|
||||
*
|
||||
* <p>The low-pass filter filters incoming rotation rate commands to smooth out high frequency
|
||||
* changes.
|
||||
*
|
||||
* @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.
|
||||
*/
|
||||
public void setQuickStopAlpha(double alpha) {
|
||||
m_quickStopAlpha = alpha;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stopMotor() {
|
||||
m_leftMotor.stopMotor();
|
||||
|
||||
@@ -28,8 +28,19 @@ import edu.wpi.first.wpilibj.SpeedController;
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
|
||||
* points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
|
||||
* positive.
|
||||
*/
|
||||
public class KilloughDrive extends RobotDriveBase {
|
||||
public static final double kDefaultLeftMotorAngle = 60.0;
|
||||
public static final double kDefaultRightMotorAngle = 120.0;
|
||||
public static final double kDefaultBackMotorAngle = 270.0;
|
||||
|
||||
private SpeedController m_leftMotor;
|
||||
private SpeedController m_rightMotor;
|
||||
private SpeedController m_backMotor;
|
||||
@@ -43,8 +54,8 @@ public class KilloughDrive extends RobotDriveBase {
|
||||
/**
|
||||
* Construct a Killough drive with the given motors and default motor angles.
|
||||
*
|
||||
* <p>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.
|
||||
* <p>The default motor angles make the wheels on each corner parallel to their respective
|
||||
* opposite sides.
|
||||
*
|
||||
* <p>If a motor needs to be inverted, do so before passing it in.
|
||||
*
|
||||
@@ -54,13 +65,14 @@ public class KilloughDrive extends RobotDriveBase {
|
||||
*/
|
||||
public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
|
||||
SpeedController backMotor) {
|
||||
this(leftMotor, rightMotor, backMotor, 120.0, 60.0, 270.0);
|
||||
this(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle, kDefaultRightMotorAngle,
|
||||
kDefaultBackMotorAngle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct a Killough drive with the given motors.
|
||||
*
|
||||
* <p>Angles are measured in counter-clockwise degrees where zero degrees is straight ahead.
|
||||
* <p>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.
|
||||
@@ -86,53 +98,55 @@ public class KilloughDrive extends RobotDriveBase {
|
||||
/**
|
||||
* 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]
|
||||
* <p>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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void driveCartesian(double x, double y, double rotation) {
|
||||
driveCartesian(x, y, rotation, 0.0);
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
|
||||
driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* <p>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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void driveCartesian(double x, double y, double rotation,
|
||||
double gyroAngle) {
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation,
|
||||
double gyroAngle) {
|
||||
if (!m_reported) {
|
||||
// HAL.report(tResourceType.kResourceType_RobotDrive, 3,
|
||||
// tInstances.kRobotDrive_KilloughCartesian);
|
||||
m_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 = new Vector2d(x, y);
|
||||
input.rotate(gyroAngle);
|
||||
Vector2d input = new Vector2d(ySpeed, xSpeed);
|
||||
input.rotate(-gyroAngle);
|
||||
|
||||
double[] wheelSpeeds = new double[3];
|
||||
wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + rotation;
|
||||
wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + rotation;
|
||||
wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + rotation;
|
||||
wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation;
|
||||
wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation;
|
||||
wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation;
|
||||
|
||||
normalize(wheelSpeeds);
|
||||
|
||||
@@ -146,24 +160,24 @@ public class KilloughDrive extends RobotDriveBase {
|
||||
/**
|
||||
* 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]
|
||||
* <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
|
||||
* drives (translation) 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.
|
||||
*/
|
||||
public void drivePolar(double magnitude, double angle, double rotation) {
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void drivePolar(double magnitude, double angle, double zRotation) {
|
||||
if (!m_reported) {
|
||||
// HAL.report(tResourceType.kResourceType_RobotDrive, 3,
|
||||
// tInstances.kRobotDrive_KilloughPolar);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
// Normalized for full power along the Cartesian axes.
|
||||
magnitude = limit(magnitude) * Math.sqrt(2.0);
|
||||
|
||||
driveCartesian(magnitude * Math.cos(angle * (Math.PI / 180.0)),
|
||||
magnitude * Math.sin(angle * (Math.PI / 180.0)), rotation, 0.0);
|
||||
driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
|
||||
magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -32,6 +32,13 @@ import edu.wpi.first.wpilibj.hal.HAL;
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
|
||||
* points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
|
||||
* positive.
|
||||
*/
|
||||
public class MecanumDrive extends RobotDriveBase {
|
||||
private SpeedController m_frontLeftMotor;
|
||||
@@ -57,49 +64,55 @@ public class MecanumDrive extends RobotDriveBase {
|
||||
/**
|
||||
* 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]
|
||||
* <p>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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void driveCartesian(double x, double y, double rotation) {
|
||||
driveCartesian(x, y, rotation, 0.0);
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
|
||||
driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* <p>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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void driveCartesian(double x, double y, double rotation, double gyroAngle) {
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
|
||||
if (!m_reported) {
|
||||
HAL.report(tResourceType.kResourceType_RobotDrive, 4,
|
||||
tInstances.kRobotDrive_MecanumCartesian);
|
||||
m_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 = new Vector2d(x, y);
|
||||
input.rotate(gyroAngle);
|
||||
Vector2d input = new Vector2d(ySpeed, xSpeed);
|
||||
input.rotate(-gyroAngle);
|
||||
|
||||
double[] wheelSpeeds = new double[4];
|
||||
wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + rotation;
|
||||
wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y + rotation;
|
||||
wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + rotation;
|
||||
wheelSpeeds[MotorType.kRearRight.value] = -input.x - input.y + rotation;
|
||||
wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
|
||||
wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y + zRotation;
|
||||
wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
|
||||
wheelSpeeds[MotorType.kRearRight.value] = -input.x - input.y + zRotation;
|
||||
|
||||
normalize(wheelSpeeds);
|
||||
|
||||
@@ -114,23 +127,23 @@ public class MecanumDrive extends RobotDriveBase {
|
||||
/**
|
||||
* 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]
|
||||
* <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
|
||||
* drives (translation) 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.
|
||||
*/
|
||||
public void drivePolar(double magnitude, double angle, double rotation) {
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void drivePolar(double magnitude, double angle, double zRotation) {
|
||||
if (!m_reported) {
|
||||
HAL.report(tResourceType.kResourceType_RobotDrive, 4, tInstances.kRobotDrive_MecanumPolar);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
// Normalized for full power along the Cartesian axes.
|
||||
magnitude = limit(magnitude) * Math.sqrt(2.0);
|
||||
|
||||
driveCartesian(magnitude * Math.cos(angle * (Math.PI / 180.0)),
|
||||
magnitude * Math.sin(angle * (Math.PI / 180.0)), rotation, 0.0);
|
||||
driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
|
||||
magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
Reference in New Issue
Block a user