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

Split RobotDrive class into a class for each drive type (#552)

Browse Source 
DiffDrive.CurvatureDrive (aka CheesyDrive) and KilloughDrive were also added.
This reorganization paves the way for SwerveDrive.
This commit is contained in:
Tyler Veness
2017-09-28 23:30:00 -07:00
committed by Peter Johnson
parent abb66d3e4b
commit 19addb04cf
18 changed files with 1790 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
View File

@@ -20,7 +20,10 @@ import static java.util.Objects.requireNonNull;
* Motor channel numbers are supplied on creation of the class. Those are used for either the drive
* function (intended for hand created drive code, such as autonomous) or with the Tank/Arcade
* functions intended to be used for Operator Control driving.
*
* @deprecated Use DifferentialDrive or MecanumDrive classes instead.
*/
@Deprecated
public class RobotDrive implements MotorSafety {
protected MotorSafetyHelper m_safetyHelper;

286
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java Normal file
View File

@@ -0,0 +1,286 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2017 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
import edu.wpi.first.wpilibj.hal.HAL;
/**
* 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 SpeedController per side. For four and
* six motor drivetrains, construct and pass in {@link SpeedControllerGroup} instances as follows.
*
* <p>Four motor drivetrain:
* <pre><code>
* public class Robot {
* Talon m_frontLeft = new Talon(1);
* Talon m_rearLeft = new Talon(2);
* SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
*
* Talon m_frontRight = new Talon(3);
* Talon m_rearRight = new Talon(4);
* SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
*
* DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
* }
* </code></pre>
*
* <p>Six motor drivetrain:
* <pre><code>
* public class Robot {
* Talon m_frontLeft = new Talon(1);
* Talon m_midLeft = new Talon(2);
* Talon m_rearLeft = new Talon(3);
* SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
*
* Talon m_frontRight = new Talon(4);
* Talon m_midRight = new Talon(5);
* Talon m_rearRight = new Talon(6);
* SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
*
* DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
* }
* </code></pre>
*
* <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. Motor outputs for the right side are negated, so motor direction inversion by the user is
* usually unnecessary.
*/
public class DifferentialDrive extends RobotDriveBase {
private SpeedController m_leftMotor;
private SpeedController m_rightMotor;
private double m_quickStopAccumulator = 0.0;
private boolean m_reported = false;
/**
* Construct a DifferentialDrive.
*
* <p>To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be
* inverted, do so before passing it in.
*/
public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) {
m_leftMotor = leftMotor;
m_rightMotor = rightMotor;
}
/**
* 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]
*/
@SuppressWarnings("ParameterName")
public void arcadeDrive(double y, double rotation) {
arcadeDrive(y, rotation, 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 squaredInputs If set, decreases the input sensitivity at low speeds.
*/
@SuppressWarnings("ParameterName")
public void arcadeDrive(double y, double rotation, 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);
rotation = limit(rotation);
rotation = applyDeadband(rotation, m_deadband);
// 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);
}
double leftMotorOutput;
double rightMotorOutput;
double maxInput = Math.copySign(Math.max(Math.abs(y), Math.abs(rotation)), y);
if (y > 0.0) {
// First quadrant, else second quadrant
if (rotation > 0.0) {
leftMotorOutput = maxInput;
rightMotorOutput = y - rotation;
} else {
leftMotorOutput = y + rotation;
rightMotorOutput = maxInput;
}
} else {
// Third quadrant, else fourth quadrant
if (rotation > 0.0) {
leftMotorOutput = y + rotation;
rightMotorOutput = maxInput;
} else {
leftMotorOutput = maxInput;
rightMotorOutput = y - rotation;
}
}
m_leftMotor.set(limit(leftMotorOutput) * m_maxOutput);
m_rightMotor.set(-limit(rightMotorOutput) * m_maxOutput);
m_safetyHelper.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. Also handles the
* 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 isQuickTurn If set, overrides constant-curvature turning for
* turn-in-place maneuvers.
*/
@SuppressWarnings("ParameterName")
public void curvatureDrive(double y, double rotation, 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);
rotation = limit(rotation);
rotation = applyDeadband(rotation, 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;
}
overPower = true;
angularPower = rotation;
} else {
overPower = false;
angularPower = Math.abs(y) * rotation - m_quickStopAccumulator;
if (m_quickStopAccumulator > 1) {
m_quickStopAccumulator -= 1;
} else if (m_quickStopAccumulator < -1) {
m_quickStopAccumulator += 1;
} else {
m_quickStopAccumulator = 0.0;
}
}
double leftMotorOutput = y + angularPower;
double rightMotorOutput = y - angularPower;
// If rotation is overpowered, reduce both outputs to within acceptable range
if (overPower) {
if (leftMotorOutput > 1.0) {
rightMotorOutput -= leftMotorOutput - 1.0;
leftMotorOutput = 1.0;
} else if (rightMotorOutput > 1.0) {
leftMotorOutput -= rightMotorOutput - 1.0;
rightMotorOutput = 1.0;
} else if (leftMotorOutput < -1.0) {
rightMotorOutput -= leftMotorOutput + 1.0;
leftMotorOutput = -1.0;
} else if (rightMotorOutput < -1.0) {
leftMotorOutput -= rightMotorOutput + 1.0;
rightMotorOutput = -1.0;
}
}
m_leftMotor.set(leftMotorOutput * m_maxOutput);
m_rightMotor.set(-rightMotorOutput * m_maxOutput);
m_safetyHelper.feed();
}
/**
* 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]
*/
public void tankDrive(double left, double right) {
tankDrive(left, right, 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 squaredInputs If set, decreases the input sensitivity at low speeds.
*/
public void tankDrive(double left, double right, 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);
right = limit(right);
right = applyDeadband(right, m_deadband);
// 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);
}
m_leftMotor.set(left * m_maxOutput);
m_rightMotor.set(-right * m_maxOutput);
m_safetyHelper.feed();
}
@Override
public void stopMotor() {
m_leftMotor.stopMotor();
m_rightMotor.stopMotor();
m_safetyHelper.feed();
}
@Override
public String getDescription() {
return "DifferentialDrive";
}
}

181
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java Normal file
View File

@@ -0,0 +1,181 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.wpilibj.SpeedController;
// import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
// import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
// import edu.wpi.first.wpilibj.hal.HAL;
/**
* 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.
*/
public class KilloughDrive extends RobotDriveBase {
private SpeedController m_leftMotor;
private SpeedController m_rightMotor;
private SpeedController m_backMotor;
private Vector2d m_leftVec;
private Vector2d m_rightVec;
private Vector2d m_backVec;
private boolean m_reported = false;
/**
* 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>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(SpeedController leftMotor, SpeedController rightMotor,
SpeedController backMotor) {
this(leftMotor, rightMotor, backMotor, 120.0, 60.0, 270.0);
}
/**
* Construct a Killough drive with the given motors.
*
* <p>Angles are measured in counter-clockwise degrees where zero degrees is straight ahead.
*
* @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(SpeedController leftMotor, SpeedController rightMotor,
SpeedController backMotor, double leftMotorAngle, double rightMotorAngle,
double backMotorAngle) {
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)));
}
/**
* 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]
*/
@SuppressWarnings("ParameterName")
public void driveCartesian(double x, double y, double rotation) {
driveCartesian(x, y, rotation, 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.
*/
@SuppressWarnings("ParameterName")
public void driveCartesian(double x, double y, double rotation,
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);
y = limit(y);
y = applyDeadband(y, m_deadband);
// Compensate for gyro angle.
Vector2d input = new Vector2d(x, y);
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;
normalize(wheelSpeeds);
m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput);
m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput);
m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * m_maxOutput);
m_safetyHelper.feed();
}
/**
* 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]
*/
public void drivePolar(double magnitude, double angle, double rotation) {
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);
}
@Override
public void stopMotor() {
m_leftMotor.stopMotor();
m_rightMotor.stopMotor();
m_backMotor.stopMotor();
m_safetyHelper.feed();
}
@Override
public String getDescription() {
return "KilloughDrive";
}
}

149
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java Normal file
View File

@@ -0,0 +1,149 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2017 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
import edu.wpi.first.wpilibj.hal.HAL;
/**
* 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. Motor outputs for the right side are negated, so motor direction inversion by the user is
* usually unnecessary.
*/
public class MecanumDrive extends RobotDriveBase {
private SpeedController m_frontLeftMotor;
private SpeedController m_rearLeftMotor;
private SpeedController m_frontRightMotor;
private SpeedController m_rearRightMotor;
private boolean m_reported = false;
/**
* Construct a MecanumDrive.
*
* <p>If a motor needs to be inverted, do so before passing it in.
*/
public MecanumDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
SpeedController frontRightMotor, SpeedController rearRightMotor) {
m_frontLeftMotor = frontLeftMotor;
m_rearLeftMotor = rearLeftMotor;
m_frontRightMotor = frontRightMotor;
m_rearRightMotor = rearRightMotor;
}
/**
* 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]
*/
@SuppressWarnings("ParameterName")
public void driveCartesian(double x, double y, double rotation) {
driveCartesian(x, y, rotation, 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.
*/
@SuppressWarnings("ParameterName")
public void driveCartesian(double x, double y, double rotation, 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);
y = limit(y);
y = applyDeadband(y, m_deadband);
// Compensate for gyro angle.
Vector2d input = new Vector2d(x, y);
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;
normalize(wheelSpeeds);
m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
m_safetyHelper.feed();
}
/**
* 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]
*/
public void drivePolar(double magnitude, double angle, double rotation) {
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);
}
@Override
public void stopMotor() {
m_frontLeftMotor.stopMotor();
m_frontRightMotor.stopMotor();
m_rearLeftMotor.stopMotor();
m_rearRightMotor.stopMotor();
m_safetyHelper.feed();
}
@Override
public String getDescription() {
return "MecanumDrive";
}
}

134
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java Normal file
View File

@@ -0,0 +1,134 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.wpilibj.MotorSafety;
import edu.wpi.first.wpilibj.MotorSafetyHelper;
/**
* Common base class for drive platforms.
*/
public abstract class RobotDriveBase implements MotorSafety {
protected double m_deadband = 0.02;
protected double m_maxOutput = 1.0;
protected MotorSafetyHelper m_safetyHelper = new MotorSafetyHelper(this);
/**
* The location of a motor on the robot for the purpose of driving.
*/
public enum MotorType {
kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3), kLeft(0),
kRight(1), kBack(2);
@SuppressWarnings("MemberName")
public final int value;
MotorType(int value) {
this.value = value;
}
}
public RobotDriveBase() {
m_safetyHelper.setSafetyEnabled(true);
}
public void setDeadband(double deadband) {
m_deadband = deadband;
}
/**
* Configure the scaling factor for using RobotDrive with motor controllers in a mode other than
* PercentVbus.
*
* @param maxOutput Multiplied with the output percentage computed by the drive functions.
*/
public void setMaxOutput(double maxOutput) {
m_maxOutput = maxOutput;
}
@Override
public void setExpiration(double timeout) {
m_safetyHelper.setExpiration(timeout);
}
@Override
public double getExpiration() {
return m_safetyHelper.getExpiration();
}
@Override
public boolean isAlive() {
return m_safetyHelper.isAlive();
}
@Override
public abstract void stopMotor();
@Override
public boolean isSafetyEnabled() {
return m_safetyHelper.isSafetyEnabled();
}
@Override
public void setSafetyEnabled(boolean enabled) {
m_safetyHelper.setSafetyEnabled(enabled);
}
@Override
public abstract String getDescription();
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
protected double limit(double value) {
if (value > 1.0) {
return 1.0;
}
if (value < -1.0) {
return -1.0;
}
return value;
}
/**
* Returns 0.0 if the given value is within the specified range around zero. The remaining range
* between the deadband and 1.0 is scaled from 0.0 to 1.0.
*
* @param value value to clip
* @param deadband range around zero
*/
protected double applyDeadband(double value, double deadband) {
if (Math.abs(value) > deadband) {
if (value > 0.0) {
return (value - deadband) / (1.0 - deadband);
} else {
return (value + deadband) / (1.0 - deadband);
}
} else {
return 0.0;
}
}
/**
* Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
*/
protected 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;
}
}
}
}

65
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java Normal file
View File

@@ -0,0 +1,65 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the 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 = 0.0;
public double y = 0.0;
public Vector2d() {}
@SuppressWarnings("ParameterName")
public Vector2d(double x, double y) {
this.x = x;
this.y = y;
}
/**
* Rotate a vector in Cartesian space.
*
* @param angle angle 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.
*/
public double dot(Vector2d vec) {
return x * vec.x + y * vec.y;
}
/**
* Returns 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.
*/
public double scalarProject(Vector2d vec) {
return dot(vec) / vec.magnitude();
}
}