mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpilib] Make drive classes follow NWU axes convention (#4079)
All trigonometric functions and vector classes assume North-West-Up axes convention, so using North-East-Down convention with them is really error-prone. We've broken something every time we touched the drive classes. We originally used North-East-Down to match the joystick convention, but the volume of long-lived bugs has made this not worth it in retrospect. The rest of WPILib also uses North-West-Up, so this makes things consistent. KilloughDrive was removed since no one uses it.
This commit is contained in:
Tyler Veness
@@ -73,12 +73,10 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController;
|
||||
* <p>Each drive function provides different inverse kinematic relations for a differential drive
|
||||
* robot.
|
||||
*
|
||||
* <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.
|
||||
* <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
|
||||
* frame). The positive X axis points ahead, the positive Y axis points to the left, and the
|
||||
* positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
|
||||
* around the Z axis is positive.
|
||||
*
|
||||
* <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
|
||||
* be set to 0, and larger values will be scaled so that the full range is still used. This deadband
|
||||
@@ -152,7 +150,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
* decrease sensitivity at low speeds.
|
||||
*
|
||||
* @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
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
*/
|
||||
public void arcadeDrive(double xSpeed, double zRotation) {
|
||||
@@ -163,7 +161,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
* Arcade drive method for differential drive platform.
|
||||
*
|
||||
* @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
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low speeds.
|
||||
*/
|
||||
@@ -192,7 +190,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
* heading change. This makes the robot more controllable at high speeds.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive.
|
||||
* @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
|
||||
* @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
|
||||
* maneuvers. zRotation will control turning rate instead of curvature.
|
||||
*/
|
||||
@@ -256,7 +254,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
* Arcade drive inverse kinematics for differential drive platform.
|
||||
*
|
||||
* @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
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param squareInputs If set, decreases the input sensitivity at low speeds.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
@@ -272,37 +270,19 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
zRotation = Math.copySign(zRotation * zRotation, zRotation);
|
||||
}
|
||||
|
||||
double leftSpeed;
|
||||
double rightSpeed;
|
||||
double leftSpeed = xSpeed - zRotation;
|
||||
double rightSpeed = xSpeed + zRotation;
|
||||
|
||||
double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
|
||||
|
||||
if (Double.compare(xSpeed, 0.0) >= 0) {
|
||||
// First quadrant, else second quadrant
|
||||
if (Double.compare(zRotation, 0.0) >= 0) {
|
||||
leftSpeed = maxInput;
|
||||
rightSpeed = xSpeed - zRotation;
|
||||
} else {
|
||||
leftSpeed = xSpeed + zRotation;
|
||||
rightSpeed = maxInput;
|
||||
}
|
||||
} else {
|
||||
// Third quadrant, else fourth quadrant
|
||||
if (Double.compare(zRotation, 0.0) >= 0) {
|
||||
leftSpeed = xSpeed + zRotation;
|
||||
rightSpeed = maxInput;
|
||||
} else {
|
||||
leftSpeed = maxInput;
|
||||
rightSpeed = xSpeed - zRotation;
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize the wheel speeds
|
||||
double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
|
||||
if (maxMagnitude > 1.0) {
|
||||
leftSpeed /= maxMagnitude;
|
||||
rightSpeed /= maxMagnitude;
|
||||
// Find the maximum possible value of (throttle + turn) along the vector
|
||||
// that the joystick is pointing, then desaturate the wheel speeds
|
||||
double greaterInput = Math.max(Math.abs(xSpeed), Math.abs(zRotation));
|
||||
double lesserInput = Math.min(Math.abs(xSpeed), Math.abs(zRotation));
|
||||
if (greaterInput == 0.0) {
|
||||
return new WheelSpeeds(0.0, 0.0);
|
||||
}
|
||||
double saturatedInput = (greaterInput + lesserInput) / greaterInput;
|
||||
leftSpeed /= saturatedInput;
|
||||
rightSpeed /= saturatedInput;
|
||||
|
||||
return new WheelSpeeds(leftSpeed, rightSpeed);
|
||||
}
|
||||
@@ -314,7 +294,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
* heading change. This makes the robot more controllable at high speeds.
|
||||
*
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive.
|
||||
* @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
|
||||
* @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
|
||||
* maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
@@ -328,14 +308,14 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
||||
double rightSpeed;
|
||||
|
||||
if (allowTurnInPlace) {
|
||||
leftSpeed = xSpeed + zRotation;
|
||||
rightSpeed = xSpeed - zRotation;
|
||||
leftSpeed = xSpeed - zRotation;
|
||||
rightSpeed = xSpeed + zRotation;
|
||||
} else {
|
||||
leftSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
|
||||
rightSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
|
||||
leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
|
||||
rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
|
||||
}
|
||||
|
||||
// Normalize wheel speeds
|
||||
// Desaturate wheel speeds
|
||||
double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
|
||||
if (maxMagnitude > 1.0) {
|
||||
leftSpeed /= maxMagnitude;
|
||||
|
||||
@@ -1,309 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.drive;
|
||||
|
||||
import static java.util.Objects.requireNonNull;
|
||||
|
||||
import edu.wpi.first.hal.FRCNetComm.tInstances;
|
||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
|
||||
|
||||
/**
|
||||
* A class for driving Killough drive platforms.
|
||||
*
|
||||
* <p>Killough drives are triangular with one omni wheel on each corner.
|
||||
*
|
||||
* <p>Drive base diagram:
|
||||
*
|
||||
* <pre>
|
||||
* /_____\
|
||||
* / \ / \
|
||||
* \ /
|
||||
* ---
|
||||
* </pre>
|
||||
*
|
||||
* <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.
|
||||
*
|
||||
* <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The driveCartesian or
|
||||
* drivePolar methods should be called periodically to avoid Motor Safety timeouts.
|
||||
*/
|
||||
public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable {
|
||||
public static final double kDefaultLeftMotorAngle = 60.0;
|
||||
public static final double kDefaultRightMotorAngle = 120.0;
|
||||
public static final double kDefaultBackMotorAngle = 270.0;
|
||||
|
||||
private static int instances;
|
||||
|
||||
private MotorController m_leftMotor;
|
||||
private MotorController m_rightMotor;
|
||||
private MotorController m_backMotor;
|
||||
|
||||
private Vector2d m_leftVec;
|
||||
private Vector2d m_rightVec;
|
||||
private Vector2d m_backVec;
|
||||
|
||||
private boolean m_reported;
|
||||
|
||||
/**
|
||||
* Wheel speeds for a Killough drive.
|
||||
*
|
||||
* <p>Uses normalized voltage [-1.0..1.0].
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public static class WheelSpeeds {
|
||||
public double left;
|
||||
public double right;
|
||||
public double back;
|
||||
|
||||
/** Constructs a WheelSpeeds with zeroes for left, right, and back speeds. */
|
||||
public WheelSpeeds() {}
|
||||
|
||||
/**
|
||||
* Constructs a WheelSpeeds.
|
||||
*
|
||||
* @param left The left speed [-1.0..1.0].
|
||||
* @param right The right speed [-1.0..1.0].
|
||||
* @param back The back speed [-1.0..1.0].
|
||||
*/
|
||||
public WheelSpeeds(double left, double right, double back) {
|
||||
this.left = left;
|
||||
this.right = right;
|
||||
this.back = back;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct a Killough drive with the given motors and default motor angles.
|
||||
*
|
||||
* <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.
|
||||
*
|
||||
* @param leftMotor The motor on the left corner.
|
||||
* @param rightMotor The motor on the right corner.
|
||||
* @param backMotor The motor on the back corner.
|
||||
*/
|
||||
public KilloughDrive(
|
||||
MotorController leftMotor, MotorController rightMotor, MotorController backMotor) {
|
||||
this(
|
||||
leftMotor,
|
||||
rightMotor,
|
||||
backMotor,
|
||||
kDefaultLeftMotorAngle,
|
||||
kDefaultRightMotorAngle,
|
||||
kDefaultBackMotorAngle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct a Killough drive with the given motors.
|
||||
*
|
||||
* <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.
|
||||
* @param backMotor The motor on the back corner.
|
||||
* @param leftMotorAngle The angle of the left wheel's forward direction of travel.
|
||||
* @param rightMotorAngle The angle of the right wheel's forward direction of travel.
|
||||
* @param backMotorAngle The angle of the back wheel's forward direction of travel.
|
||||
*/
|
||||
public KilloughDrive(
|
||||
MotorController leftMotor,
|
||||
MotorController rightMotor,
|
||||
MotorController backMotor,
|
||||
double leftMotorAngle,
|
||||
double rightMotorAngle,
|
||||
double backMotorAngle) {
|
||||
requireNonNull(leftMotor, "Left motor cannot be null");
|
||||
requireNonNull(rightMotor, "Right motor cannot be null");
|
||||
requireNonNull(backMotor, "Back motor cannot be null");
|
||||
|
||||
m_leftMotor = leftMotor;
|
||||
m_rightMotor = rightMotor;
|
||||
m_backMotor = backMotor;
|
||||
m_leftVec =
|
||||
new Vector2d(
|
||||
Math.cos(leftMotorAngle * (Math.PI / 180.0)),
|
||||
Math.sin(leftMotorAngle * (Math.PI / 180.0)));
|
||||
m_rightVec =
|
||||
new Vector2d(
|
||||
Math.cos(rightMotorAngle * (Math.PI / 180.0)),
|
||||
Math.sin(rightMotorAngle * (Math.PI / 180.0)));
|
||||
m_backVec =
|
||||
new Vector2d(
|
||||
Math.cos(backMotorAngle * (Math.PI / 180.0)),
|
||||
Math.sin(backMotorAngle * (Math.PI / 180.0)));
|
||||
SendableRegistry.addChild(this, m_leftMotor);
|
||||
SendableRegistry.addChild(this, m_rightMotor);
|
||||
SendableRegistry.addChild(this, m_backMotor);
|
||||
instances++;
|
||||
SendableRegistry.addLW(this, "KilloughDrive", instances);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() {
|
||||
SendableRegistry.remove(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Killough platform.
|
||||
*
|
||||
* <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.
|
||||
*/
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
|
||||
driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Killough platform.
|
||||
*
|
||||
* <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.
|
||||
*/
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
|
||||
if (!m_reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughCartesian, 3);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
|
||||
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
|
||||
|
||||
var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
|
||||
|
||||
m_leftMotor.set(speeds.left * m_maxOutput);
|
||||
m_rightMotor.set(speeds.right * m_maxOutput);
|
||||
m_backMotor.set(speeds.back * m_maxOutput);
|
||||
|
||||
feed();
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Killough platform.
|
||||
*
|
||||
* <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 zRotation) {
|
||||
if (!m_reported) {
|
||||
HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughPolar, 3);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
driveCartesian(
|
||||
magnitude * Math.sin(angle * (Math.PI / 180.0)),
|
||||
magnitude * Math.cos(angle * (Math.PI / 180.0)),
|
||||
zRotation,
|
||||
0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Cartesian inverse kinematics for Killough platform.
|
||||
*
|
||||
* <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.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
*/
|
||||
public WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation) {
|
||||
return driveCartesianIK(ySpeed, xSpeed, zRotation, 0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Cartesian inverse kinematics for Killough platform.
|
||||
*
|
||||
* <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.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
*/
|
||||
public WheelSpeeds driveCartesianIK(
|
||||
double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
|
||||
ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
|
||||
xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
|
||||
|
||||
// Compensate for gyro angle.
|
||||
Vector2d input = new Vector2d(ySpeed, xSpeed);
|
||||
input.rotate(-gyroAngle);
|
||||
|
||||
double[] wheelSpeeds = new double[3];
|
||||
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);
|
||||
|
||||
return new WheelSpeeds(
|
||||
wheelSpeeds[MotorType.kLeft.value],
|
||||
wheelSpeeds[MotorType.kRight.value],
|
||||
wheelSpeeds[MotorType.kBack.value]);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stopMotor() {
|
||||
m_leftMotor.stopMotor();
|
||||
m_rightMotor.stopMotor();
|
||||
m_backMotor.stopMotor();
|
||||
feed();
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getDescription() {
|
||||
return "KilloughDrive";
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("KilloughDrive");
|
||||
builder.setActuator(true);
|
||||
builder.setSafeState(this::stopMotor);
|
||||
builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
|
||||
builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set);
|
||||
builder.addDoubleProperty("Back Motor Speed", m_backMotor::get, m_backMotor::set);
|
||||
}
|
||||
}
|
||||
@@ -10,6 +10,8 @@ import edu.wpi.first.hal.FRCNetComm.tInstances;
|
||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
@@ -36,9 +38,10 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController;
|
||||
* <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
|
||||
* robot.
|
||||
*
|
||||
* <p>The positive Y axis points ahead, the positive X 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.
|
||||
* <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
|
||||
* frame). The positive X axis points ahead, the positive Y axis points to the left, and the
|
||||
* positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
|
||||
* around the Z axis is positive.
|
||||
*
|
||||
* <p>Note: the axis conventions used in this class differ from DifferentialDrive. This may change
|
||||
* in a future year's WPILib release.
|
||||
@@ -131,42 +134,42 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
/**
|
||||
* Drive method for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
|
||||
* from its angle or rotation rate.
|
||||
* <p>Angles are measured counterclockwise 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]. Forward is positive.
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
*/
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
|
||||
driveCartesian(ySpeed, xSpeed, zRotation, 0.0);
|
||||
public void driveCartesian(double xSpeed, double ySpeed, double zRotation) {
|
||||
driveCartesian(xSpeed, ySpeed, zRotation, new Rotation2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
|
||||
* from its angle or rotation rate.
|
||||
* <p>Angles are measured counterclockwise 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]. Forward is positive.
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
|
||||
* @param xSpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the X axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
|
||||
* to implement field-oriented controls.
|
||||
* @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
|
||||
* controls.
|
||||
*/
|
||||
public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
|
||||
public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
|
||||
if (!m_reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumCartesian, 4);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
|
||||
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
|
||||
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
|
||||
|
||||
var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
|
||||
var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
|
||||
|
||||
m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
|
||||
m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
|
||||
@@ -179,41 +182,38 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
/**
|
||||
* Drive method for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
|
||||
* <p>Angles are measured counterclockwise 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
|
||||
* @param angle The gyro heading around the Z axis at which the robot drives.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
*/
|
||||
public void drivePolar(double magnitude, double angle, double zRotation) {
|
||||
public void drivePolar(double magnitude, Rotation2d angle, double zRotation) {
|
||||
if (!m_reported) {
|
||||
HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumPolar, 4);
|
||||
m_reported = true;
|
||||
}
|
||||
|
||||
driveCartesian(
|
||||
magnitude * Math.cos(angle * (Math.PI / 180.0)),
|
||||
magnitude * Math.sin(angle * (Math.PI / 180.0)),
|
||||
zRotation,
|
||||
0.0);
|
||||
magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, new Rotation2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Cartesian inverse kinematics for Mecanum platform.
|
||||
*
|
||||
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
|
||||
* from its angle or rotation rate.
|
||||
* <p>Angles are measured counterclockwise 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]. Forward is positive.
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation) {
|
||||
return driveCartesianIK(ySpeed, xSpeed, zRotation, 0.0);
|
||||
public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) {
|
||||
return driveCartesianIK(xSpeed, ySpeed, zRotation, new Rotation2d());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -222,28 +222,27 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
||||
* <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]. Forward is positive.
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
|
||||
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
|
||||
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Left is positive.
|
||||
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
|
||||
* positive.
|
||||
* @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
|
||||
* to implement field-oriented controls.
|
||||
* @param gyroAngle The gyro heading around the Z axis. Use this to implement field-oriented
|
||||
* controls.
|
||||
* @return Wheel speeds [-1.0..1.0].
|
||||
*/
|
||||
public static WheelSpeeds driveCartesianIK(
|
||||
double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
|
||||
ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
|
||||
double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
|
||||
xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
|
||||
ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
|
||||
|
||||
// Compensate for gyro angle.
|
||||
Vector2d input = new Vector2d(ySpeed, xSpeed);
|
||||
input.rotate(-gyroAngle);
|
||||
var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());
|
||||
|
||||
double[] wheelSpeeds = new double[4];
|
||||
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;
|
||||
wheelSpeeds[MotorType.kFrontLeft.value] = input.getX() + input.getY() + zRotation;
|
||||
wheelSpeeds[MotorType.kFrontRight.value] = input.getX() - input.getY() - zRotation;
|
||||
wheelSpeeds[MotorType.kRearLeft.value] = input.getX() - input.getY() + zRotation;
|
||||
wheelSpeeds[MotorType.kRearRight.value] = input.getX() + input.getY() - zRotation;
|
||||
|
||||
normalize(wheelSpeeds);
|
||||
|
||||
|
||||
@@ -1,63 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.drive;
|
||||
|
||||
/** This is a 2D vector struct that supports basic vector operations. */
|
||||
@SuppressWarnings("MemberName")
|
||||
public class Vector2d {
|
||||
public double x;
|
||||
public double y;
|
||||
|
||||
public Vector2d() {}
|
||||
|
||||
public Vector2d(double x, double y) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotate a vector in Cartesian space.
|
||||
*
|
||||
* @param angle angle in degrees by which to rotate vector counter-clockwise.
|
||||
*/
|
||||
public void rotate(double angle) {
|
||||
double cosA = Math.cos(angle * (Math.PI / 180.0));
|
||||
double sinA = Math.sin(angle * (Math.PI / 180.0));
|
||||
double[] out = new double[2];
|
||||
out[0] = x * cosA - y * sinA;
|
||||
out[1] = x * sinA + y * cosA;
|
||||
x = out[0];
|
||||
y = out[1];
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns dot product of this vector with argument.
|
||||
*
|
||||
* @param vec Vector with which to perform dot product.
|
||||
* @return Dot product of this vector with argument.
|
||||
*/
|
||||
public double dot(Vector2d vec) {
|
||||
return x * vec.x + y * vec.y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns magnitude of vector.
|
||||
*
|
||||
* @return Magnitude of vector.
|
||||
*/
|
||||
public double magnitude() {
|
||||
return Math.sqrt(x * x + y * y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns scalar projection of this vector onto argument.
|
||||
*
|
||||
* @param vec Vector onto which to project this vector.
|
||||
* @return scalar projection of this vector onto argument.
|
||||
*/
|
||||
public double scalarProject(Vector2d vec) {
|
||||
return dot(vec) / vec.magnitude();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user