Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT Move java files

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 19:55:40 -05:00
committed by Peter Johnson
parent 7ca1be9bae
commit c350c5f112
1486 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

368
wpilibj/src/main/java/org/wpilib/drive/DifferentialDrive.java Normal file
View File

@@ -0,0 +1,368 @@
// 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 edu.wpi.first.util.ErrorMessages.requireNonNullParam;
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;
import java.util.function.DoubleConsumer;
/**
* A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
* base, "tank drive", or West Coast Drive.
*
* <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
* (e.g., 6WD or 8WD). This class takes a setter per side. For four and six motor drivetrains, use
* CAN motor controller followers or {@link
* edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}.
*
* <p>A differential drive robot has left and right wheels separated by an arbitrary width.
*
* <p>Drive base diagram:
*
* <pre>
* |_______|
* | | | |
* | |
* |_|___|_|
* | |
* </pre>
*
* <p>Each drive function provides different inverse kinematic relations for a differential drive
* robot.
*
* <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
* value can be changed with {@link #setDeadband}.
*
* <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The tankDrive, arcadeDrive,
* or curvatureDrive methods should be called periodically to avoid Motor Safety timeouts.
*/
public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
private static int instances;
private final DoubleConsumer m_leftMotor;
private final DoubleConsumer m_rightMotor;
// Used for Sendable property getters
private double m_leftOutput;
private double m_rightOutput;
private boolean m_reported;
/**
* Wheel speeds for a differential drive.
*
* <p>Uses normalized voltage [-1.0..1.0].
*/
@SuppressWarnings("MemberName")
public static class WheelSpeeds {
/** Left wheel speed. */
public double left;
/** Right wheel speed. */
public double right;
/** Constructs a WheelSpeeds with zeroes for left and right speeds. */
public WheelSpeeds() {}
/**
* Constructs a WheelSpeeds.
*
* @param left The left speed [-1.0..1.0].
* @param right The right speed [-1.0..1.0].
*/
public WheelSpeeds(double left, double right) {
this.left = left;
this.right = right;
}
}
/**
* Construct a DifferentialDrive.
*
* <p>To pass multiple motors per side, use CAN motor controller followers or {@link
* edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
* motor needs to be inverted, do so before passing it in.
*
* @param leftMotor Left motor.
* @param rightMotor Right motor.
*/
@SuppressWarnings({"removal", "this-escape"})
public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
this((double output) -> leftMotor.set(output), (double output) -> rightMotor.set(output));
SendableRegistry.addChild(this, leftMotor);
SendableRegistry.addChild(this, rightMotor);
}
/**
* Construct a DifferentialDrive.
*
* <p>To pass multiple motors per side, use CAN motor controller followers or {@link
* edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
* motor needs to be inverted, do so before passing it in.
*
* @param leftMotor Left motor setter.
* @param rightMotor Right motor setter.
*/
@SuppressWarnings("this-escape")
public DifferentialDrive(DoubleConsumer leftMotor, DoubleConsumer rightMotor) {
requireNonNullParam(leftMotor, "leftMotor", "DifferentialDrive");
requireNonNullParam(rightMotor, "rightMotor", "DifferentialDrive");
m_leftMotor = leftMotor;
m_rightMotor = rightMotor;
instances++;
SendableRegistry.add(this, "DifferentialDrive", instances);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Arcade drive method for differential drive platform. The calculated values will be squared to
* 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]. Counterclockwise is
* positive.
*/
public void arcadeDrive(double xSpeed, double zRotation) {
arcadeDrive(xSpeed, zRotation, true);
}
/**
* 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]. Counterclockwise is
* positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
*/
public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
if (!m_reported) {
HAL.reportUsage("RobotDrive", "DifferentialArcade");
m_reported = true;
}
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
m_leftOutput = speeds.left * m_maxOutput;
m_rightOutput = speeds.right * m_maxOutput;
m_leftMotor.accept(m_leftOutput);
m_rightMotor.accept(m_rightOutput);
feed();
}
/**
* Curvature drive method for differential drive platform.
*
* <p>The rotation argument controls the curvature of the robot's path rather than its rate of
* 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]. Counterclockwise is positive.
* @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
* maneuvers. zRotation will control turning rate instead of curvature.
*/
public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
if (!m_reported) {
HAL.reportUsage("RobotDrive", "DifferentialCurvature");
m_reported = true;
}
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
m_leftOutput = speeds.left * m_maxOutput;
m_rightOutput = speeds.right * m_maxOutput;
m_leftMotor.accept(m_leftOutput);
m_rightMotor.accept(m_rightOutput);
feed();
}
/**
* Tank drive method for differential drive platform. The calculated values will be squared to
* decrease sensitivity at low speeds.
*
* @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 leftSpeed, double rightSpeed) {
tankDrive(leftSpeed, rightSpeed, true);
}
/**
* Tank drive method for differential drive platform.
*
* @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 squareInputs If set, decreases the input sensitivity at low speeds.
*/
public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
if (!m_reported) {
HAL.reportUsage("RobotDrive", "DifferentialTank");
m_reported = true;
}
leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
m_leftOutput = speeds.left * m_maxOutput;
m_rightOutput = speeds.right * m_maxOutput;
m_leftMotor.accept(m_leftOutput);
m_rightMotor.accept(m_rightOutput);
feed();
}
/**
* 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]. Counterclockwise is
* positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
* @return Wheel speeds [-1.0..1.0].
*/
public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
zRotation = Math.clamp(zRotation, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
xSpeed = MathUtil.copyDirectionPow(xSpeed, 2);
zRotation = MathUtil.copyDirectionPow(zRotation, 2);
}
double leftSpeed = xSpeed - zRotation;
double rightSpeed = xSpeed + zRotation;
// 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);
}
/**
* Curvature drive inverse kinematics for differential drive platform.
*
* <p>The rotation argument controls the curvature of the robot's path rather than its rate of
* 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]. 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].
*/
public static WheelSpeeds curvatureDriveIK(
double xSpeed, double zRotation, boolean allowTurnInPlace) {
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
zRotation = Math.clamp(zRotation, -1.0, 1.0);
double leftSpeed;
double rightSpeed;
if (allowTurnInPlace) {
leftSpeed = xSpeed - zRotation;
rightSpeed = xSpeed + zRotation;
} else {
leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
}
// Desaturate wheel speeds
double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
if (maxMagnitude > 1.0) {
leftSpeed /= maxMagnitude;
rightSpeed /= maxMagnitude;
}
return new WheelSpeeds(leftSpeed, rightSpeed);
}
/**
* Tank drive inverse kinematics for differential drive platform.
*
* @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 squareInputs If set, decreases the input sensitivity at low speeds.
* @return Wheel speeds [-1.0..1.0].
*/
public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
leftSpeed = Math.clamp(leftSpeed, -1.0, 1.0);
rightSpeed = Math.clamp(rightSpeed, -1.0, 1.0);
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
leftSpeed = MathUtil.copyDirectionPow(leftSpeed, 2);
rightSpeed = MathUtil.copyDirectionPow(rightSpeed, 2);
}
return new WheelSpeeds(leftSpeed, rightSpeed);
}
@Override
public void stopMotor() {
m_leftOutput = 0.0;
m_rightOutput = 0.0;
m_leftMotor.accept(0.0);
m_rightMotor.accept(0.0);
feed();
}
@Override
public String getDescription() {
return "DifferentialDrive";
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("DifferentialDrive");
builder.setActuator(true);
builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor);
builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor);
}
}

323
wpilibj/src/main/java/org/wpilib/drive/MecanumDrive.java Normal file
View File

@@ -0,0 +1,323 @@
// 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 edu.wpi.first.util.ErrorMessages.requireNonNullParam;
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;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import java.util.function.DoubleConsumer;
/**
* A class for driving Mecanum drive platforms.
*
* <p>Mecanum drives are rectangular with one wheel on each corner. Each wheel has rollers toed in
* 45 degrees toward the front or back. When looking at the wheels from the top, the roller axles
* should form an X across the robot. Each drive() function provides different inverse kinematic
* relations for a Mecanum drive robot.
*
* <p>Drive base diagram:
*
* <pre>
* \\_______/
* \\ | | /
* | |
* /_|___|_\\
* / \\
* </pre>
*
* <p>Each drive() function provides different inverse kinematic relations for a Mecanum drive
* robot.
*
* <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
* value can be changed with {@link #setDeadband}.
*
* <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 MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
private static int instances;
private final DoubleConsumer m_frontLeftMotor;
private final DoubleConsumer m_rearLeftMotor;
private final DoubleConsumer m_frontRightMotor;
private final DoubleConsumer m_rearRightMotor;
// Used for Sendable property getters
private double m_frontLeftOutput;
private double m_rearLeftOutput;
private double m_frontRightOutput;
private double m_rearRightOutput;
private boolean m_reported;
/**
* Wheel speeds for a mecanum drive.
*
* <p>Uses normalized voltage [-1.0..1.0].
*/
@SuppressWarnings("MemberName")
public static class WheelSpeeds {
/** Front-left wheel speed. */
public double frontLeft;
/** Front-right wheel speed. */
public double frontRight;
/** Rear-left wheel speed. */
public double rearLeft;
/** Rear-right wheel speed. */
public double rearRight;
/** Constructs a WheelSpeeds with zeroes for all four speeds. */
public WheelSpeeds() {}
/**
* Constructs a WheelSpeeds.
*
* @param frontLeft The front left speed [-1.0..1.0].
* @param frontRight The front right speed [-1.0..1.0].
* @param rearLeft The rear left speed [-1.0..1.0].
* @param rearRight The rear right speed [-1.0..1.0].
*/
public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
}
/**
* Construct a MecanumDrive.
*
* <p>If a motor needs to be inverted, do so before passing it in.
*
* @param frontLeftMotor The motor on the front-left corner.
* @param rearLeftMotor The motor on the rear-left corner.
* @param frontRightMotor The motor on the front-right corner.
* @param rearRightMotor The motor on the rear-right corner.
*/
@SuppressWarnings({"removal", "this-escape"})
public MecanumDrive(
MotorController frontLeftMotor,
MotorController rearLeftMotor,
MotorController frontRightMotor,
MotorController rearRightMotor) {
this(
(double output) -> frontLeftMotor.set(output),
(double output) -> rearLeftMotor.set(output),
(double output) -> frontRightMotor.set(output),
(double output) -> rearRightMotor.set(output));
SendableRegistry.addChild(this, frontLeftMotor);
SendableRegistry.addChild(this, rearLeftMotor);
SendableRegistry.addChild(this, frontRightMotor);
SendableRegistry.addChild(this, rearRightMotor);
}
/**
* Construct a MecanumDrive.
*
* <p>If a motor needs to be inverted, do so before passing it in.
*
* @param frontLeftMotor The setter for the motor on the front-left corner.
* @param rearLeftMotor The setter for the motor on the rear-left corner.
* @param frontRightMotor The setter for the motor on the front-right corner.
* @param rearRightMotor The setter for the motor on the rear-right corner.
*/
@SuppressWarnings("this-escape")
public MecanumDrive(
DoubleConsumer frontLeftMotor,
DoubleConsumer rearLeftMotor,
DoubleConsumer frontRightMotor,
DoubleConsumer rearRightMotor) {
requireNonNullParam(frontLeftMotor, "frontLeftMotor", "MecanumDrive");
requireNonNullParam(rearLeftMotor, "rearLeftMotor", "MecanumDrive");
requireNonNullParam(frontRightMotor, "frontRightMotor", "MecanumDrive");
requireNonNullParam(rearRightMotor, "rearRightMotor", "MecanumDrive");
m_frontLeftMotor = frontLeftMotor;
m_rearLeftMotor = rearLeftMotor;
m_frontRightMotor = frontRightMotor;
m_rearRightMotor = rearRightMotor;
instances++;
SendableRegistry.add(this, "MecanumDrive", instances);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Drive method for Mecanum platform.
*
* <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
* independent of its angle or rotation rate.
*
* @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 xSpeed, double ySpeed, double zRotation) {
driveCartesian(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
}
/**
* Drive method for Mecanum platform.
*
* <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
* independent of its angle or rotation rate.
*
* @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 gyro heading around the Z axis. Use this to implement field-oriented
* controls.
*/
public void driveCartesian(double xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
if (!m_reported) {
HAL.reportUsage("RobotDrive", "MecanumCartesian");
m_reported = true;
}
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
m_frontLeftOutput = speeds.frontLeft * m_maxOutput;
m_rearLeftOutput = speeds.rearLeft * m_maxOutput;
m_frontRightOutput = speeds.frontRight * m_maxOutput;
m_rearRightOutput = speeds.rearRight * m_maxOutput;
m_frontLeftMotor.accept(m_frontLeftOutput);
m_frontRightMotor.accept(m_frontRightOutput);
m_rearLeftMotor.accept(m_rearLeftOutput);
m_rearRightMotor.accept(m_rearRightOutput);
feed();
}
/**
* Drive method for Mecanum platform.
*
* <p>Angles are measured counterclockwise from straight ahead. The speed at which the robot
* drives (translation) is independent of 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 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, Rotation2d angle, double zRotation) {
if (!m_reported) {
HAL.reportUsage("RobotDrive", "MecanumPolar");
m_reported = true;
}
driveCartesian(
magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, Rotation2d.kZero);
}
/**
* Cartesian inverse kinematics for Mecanum platform.
*
* <p>Angles are measured counterclockwise from the positive X axis. The robot's speed is
* independent of its angle or rotation rate.
*
* @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 xSpeed, double ySpeed, double zRotation) {
return driveCartesianIK(xSpeed, ySpeed, zRotation, Rotation2d.kZero);
}
/**
* Cartesian inverse kinematics for Mecanum platform.
*
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent of
* its angle or rotation rate.
*
* @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 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 xSpeed, double ySpeed, double zRotation, Rotation2d gyroAngle) {
xSpeed = Math.clamp(xSpeed, -1.0, 1.0);
ySpeed = Math.clamp(ySpeed, -1.0, 1.0);
// Compensate for gyro angle.
var input = new Translation2d(xSpeed, ySpeed).rotateBy(gyroAngle.unaryMinus());
double[] wheelSpeeds = new double[4];
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);
return new WheelSpeeds(
wheelSpeeds[MotorType.kFrontLeft.value],
wheelSpeeds[MotorType.kFrontRight.value],
wheelSpeeds[MotorType.kRearLeft.value],
wheelSpeeds[MotorType.kRearRight.value]);
}
@Override
public void stopMotor() {
m_frontLeftOutput = 0.0;
m_frontRightOutput = 0.0;
m_rearLeftOutput = 0.0;
m_rearRightOutput = 0.0;
m_frontLeftMotor.accept(0.0);
m_frontRightMotor.accept(0.0);
m_rearLeftMotor.accept(0.0);
m_rearRightMotor.accept(0.0);
feed();
}
@Override
public String getDescription() {
return "MecanumDrive";
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("MecanumDrive");
builder.setActuator(true);
builder.addDoubleProperty("Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor);
builder.addDoubleProperty(
"Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor);
builder.addDoubleProperty("Rear Left Motor Speed", () -> m_rearLeftOutput, m_rearLeftMotor);
builder.addDoubleProperty("Rear Right Motor Speed", () -> m_rearRightOutput, m_rearRightMotor);
}
}

117
wpilibj/src/main/java/org/wpilib/drive/RobotDriveBase.java Normal file
View File

@@ -0,0 +1,117 @@
// 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 edu.wpi.first.wpilibj.MotorSafety;
/**
* Common base class for drive platforms.
*
* <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default.
*/
public abstract class RobotDriveBase extends MotorSafety {
/** Default input deadband. */
public static final double kDefaultDeadband = 0.02;
/** Default maximum output. */
public static final double kDefaultMaxOutput = 1.0;
/** Input deadband. */
protected double m_deadband = kDefaultDeadband;
/** Maximum output. */
protected double m_maxOutput = kDefaultMaxOutput;
/** The location of a motor on the robot for the purpose of driving. */
public enum MotorType {
/** Front left motor. */
kFrontLeft(0),
/** Front right motor. */
kFrontRight(1),
/** Rear left motor. */
kRearLeft(2),
/** Rear right motor. */
kRearRight(3),
/** Left motor. */
kLeft(0),
/** Right motor. */
kRight(1),
/** Back motor. */
kBack(2);
/** MotorType value. */
public final int value;
MotorType(int value) {
this.value = value;
}
}
/** RobotDriveBase constructor. */
@SuppressWarnings("this-escape")
public RobotDriveBase() {
setSafetyEnabled(true);
}
/**
* Sets the deadband applied to the drive inputs (e.g., joystick values).
*
* <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
* 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link
* edu.wpi.first.math.MathUtil#applyDeadband}.
*
* @param deadband The deadband to set.
*/
public void setDeadband(double deadband) {
m_deadband = deadband;
}
/**
* Configure the scaling factor for using drive methods with motor controllers in a mode other
* than PercentVbus or to limit the maximum output.
*
* <p>The default value is {@value #kDefaultMaxOutput}.
*
* @param maxOutput Multiplied with the output percentage computed by the drive functions.
*/
public void setMaxOutput(double maxOutput) {
m_maxOutput = maxOutput;
}
/**
* Feed the motor safety object. Resets the timer that will stop the motors if it completes.
*
* @see MotorSafety#feed()
*/
public void feedWatchdog() {
feed();
}
@Override
public abstract void stopMotor();
@Override
public abstract String getDescription();
/**
* Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
*
* @param wheelSpeeds List of wheel speeds to normalize.
*/
protected static void normalize(double[] wheelSpeeds) {
double maxMagnitude = Math.abs(wheelSpeeds[0]);
for (int i = 1; i < wheelSpeeds.length; i++) {
double temp = Math.abs(wheelSpeeds[i]);
if (maxMagnitude < temp) {
maxMagnitude = temp;
}
}
if (maxMagnitude > 1.0) {
for (int i = 0; i < wheelSpeeds.length; i++) {
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
}
}
}
}