Source code

001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.drive;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.hal.FRCNetComm.tInstances;
010import edu.wpi.first.hal.FRCNetComm.tResourceType;
011import edu.wpi.first.hal.HAL;
012import edu.wpi.first.math.MathUtil;
013import edu.wpi.first.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import edu.wpi.first.wpilibj.motorcontrol.MotorController;
017import java.util.function.DoubleConsumer;
018
019/**
020 * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
021 * base, "tank drive", or West Coast Drive.
022 *
023 * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
024 * (e.g., 6WD or 8WD). This class takes a setter per side. For four and six motor drivetrains, use
025 * CAN motor controller followers or {@link
026 * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}.
027 *
028 * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
029 *
030 * <p>Drive base diagram:
031 *
032 * <pre>
033 * |_______|
034 * | |   | |
035 *   |   |
036 * |_|___|_|
037 * |       |
038 * </pre>
039 *
040 * <p>Each drive function provides different inverse kinematic relations for a differential drive
041 * robot.
042 *
043 * <p>This library uses the NWU axes convention (North-West-Up as external reference in the world
044 * frame). The positive X axis points ahead, the positive Y axis points to the left, and the
045 * positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation
046 * around the Z axis is positive.
047 *
048 * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
049 * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
050 * value can be changed with {@link #setDeadband}.
051 *
052 * <p>{@link edu.wpi.first.wpilibj.MotorSafety} is enabled by default. The tankDrive, arcadeDrive,
053 * or curvatureDrive methods should be called periodically to avoid Motor Safety timeouts.
054 */
055public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
056  private static int instances;
057
058  private final DoubleConsumer m_leftMotor;
059  private final DoubleConsumer m_rightMotor;
060
061  // Used for Sendable property getters
062  private double m_leftOutput;
063  private double m_rightOutput;
064
065  private boolean m_reported;
066
067  /**
068   * Wheel speeds for a differential drive.
069   *
070   * <p>Uses normalized voltage [-1.0..1.0].
071   */
072  @SuppressWarnings("MemberName")
073  public static class WheelSpeeds {
074    /** Left wheel speed. */
075    public double left;
076
077    /** Right wheel speed. */
078    public double right;
079
080    /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
081    public WheelSpeeds() {}
082
083    /**
084     * Constructs a WheelSpeeds.
085     *
086     * @param left The left speed [-1.0..1.0].
087     * @param right The right speed [-1.0..1.0].
088     */
089    public WheelSpeeds(double left, double right) {
090      this.left = left;
091      this.right = right;
092    }
093  }
094
095  /**
096   * Construct a DifferentialDrive.
097   *
098   * <p>To pass multiple motors per side, use CAN motor controller followers or {@link
099   * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
100   * motor needs to be inverted, do so before passing it in.
101   *
102   * @param leftMotor Left motor.
103   * @param rightMotor Right motor.
104   */
105  @SuppressWarnings({"removal", "this-escape"})
106  public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
107    this((double output) -> leftMotor.set(output), (double output) -> rightMotor.set(output));
108    SendableRegistry.addChild(this, leftMotor);
109    SendableRegistry.addChild(this, rightMotor);
110  }
111
112  /**
113   * Construct a DifferentialDrive.
114   *
115   * <p>To pass multiple motors per side, use CAN motor controller followers or {@link
116   * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
117   * motor needs to be inverted, do so before passing it in.
118   *
119   * @param leftMotor Left motor setter.
120   * @param rightMotor Right motor setter.
121   */
122  @SuppressWarnings("this-escape")
123  public DifferentialDrive(DoubleConsumer leftMotor, DoubleConsumer rightMotor) {
124    requireNonNullParam(leftMotor, "leftMotor", "DifferentialDrive");
125    requireNonNullParam(rightMotor, "rightMotor", "DifferentialDrive");
126
127    m_leftMotor = leftMotor;
128    m_rightMotor = rightMotor;
129    instances++;
130    SendableRegistry.addLW(this, "DifferentialDrive", instances);
131  }
132
133  @Override
134  public void close() {
135    SendableRegistry.remove(this);
136  }
137
138  /**
139   * Arcade drive method for differential drive platform. The calculated values will be squared to
140   * decrease sensitivity at low speeds.
141   *
142   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
143   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
144   *     positive.
145   */
146  public void arcadeDrive(double xSpeed, double zRotation) {
147    arcadeDrive(xSpeed, zRotation, true);
148  }
149
150  /**
151   * Arcade drive method for differential drive platform.
152   *
153   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
154   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
155   *     positive.
156   * @param squareInputs If set, decreases the input sensitivity at low speeds.
157   */
158  public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
159    if (!m_reported) {
160      HAL.report(
161          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialArcade, 2);
162      m_reported = true;
163    }
164
165    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
166    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
167
168    var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
169
170    m_leftOutput = speeds.left * m_maxOutput;
171    m_rightOutput = speeds.right * m_maxOutput;
172
173    m_leftMotor.accept(m_leftOutput);
174    m_rightMotor.accept(m_rightOutput);
175
176    feed();
177  }
178
179  /**
180   * Curvature drive method for differential drive platform.
181   *
182   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
183   * heading change. This makes the robot more controllable at high speeds.
184   *
185   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
186   * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
187   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
188   *     maneuvers. zRotation will control turning rate instead of curvature.
189   */
190  public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
191    if (!m_reported) {
192      HAL.report(
193          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialCurvature, 2);
194      m_reported = true;
195    }
196
197    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
198    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
199
200    var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
201
202    m_leftOutput = speeds.left * m_maxOutput;
203    m_rightOutput = speeds.right * m_maxOutput;
204
205    m_leftMotor.accept(m_leftOutput);
206    m_rightMotor.accept(m_rightOutput);
207
208    feed();
209  }
210
211  /**
212   * Tank drive method for differential drive platform. The calculated values will be squared to
213   * decrease sensitivity at low speeds.
214   *
215   * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is positive.
216   * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
217   *     positive.
218   */
219  public void tankDrive(double leftSpeed, double rightSpeed) {
220    tankDrive(leftSpeed, rightSpeed, true);
221  }
222
223  /**
224   * Tank drive method for differential drive platform.
225   *
226   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
227   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
228   *     positive.
229   * @param squareInputs If set, decreases the input sensitivity at low speeds.
230   */
231  public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
232    if (!m_reported) {
233      HAL.report(
234          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialTank, 2);
235      m_reported = true;
236    }
237
238    leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
239    rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
240
241    var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
242
243    m_leftOutput = speeds.left * m_maxOutput;
244    m_rightOutput = speeds.right * m_maxOutput;
245
246    m_leftMotor.accept(m_leftOutput);
247    m_rightMotor.accept(m_rightOutput);
248
249    feed();
250  }
251
252  /**
253   * Arcade drive inverse kinematics for differential drive platform.
254   *
255   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
256   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Counterclockwise is
257   *     positive.
258   * @param squareInputs If set, decreases the input sensitivity at low speeds.
259   * @return Wheel speeds [-1.0..1.0].
260   */
261  public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
262    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
263    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
264
265    // Square the inputs (while preserving the sign) to increase fine control
266    // while permitting full power.
267    if (squareInputs) {
268      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
269      zRotation = Math.copySign(zRotation * zRotation, zRotation);
270    }
271
272    double leftSpeed = xSpeed - zRotation;
273    double rightSpeed = xSpeed + zRotation;
274
275    // Find the maximum possible value of (throttle + turn) along the vector
276    // that the joystick is pointing, then desaturate the wheel speeds
277    double greaterInput = Math.max(Math.abs(xSpeed), Math.abs(zRotation));
278    double lesserInput = Math.min(Math.abs(xSpeed), Math.abs(zRotation));
279    if (greaterInput == 0.0) {
280      return new WheelSpeeds(0.0, 0.0);
281    }
282    double saturatedInput = (greaterInput + lesserInput) / greaterInput;
283    leftSpeed /= saturatedInput;
284    rightSpeed /= saturatedInput;
285
286    return new WheelSpeeds(leftSpeed, rightSpeed);
287  }
288
289  /**
290   * Curvature drive inverse kinematics for differential drive platform.
291   *
292   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
293   * heading change. This makes the robot more controllable at high speeds.
294   *
295   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
296   * @param zRotation The normalized curvature [-1.0..1.0]. Counterclockwise is positive.
297   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
298   *     maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
299   * @return Wheel speeds [-1.0..1.0].
300   */
301  public static WheelSpeeds curvatureDriveIK(
302      double xSpeed, double zRotation, boolean allowTurnInPlace) {
303    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
304    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
305
306    double leftSpeed;
307    double rightSpeed;
308
309    if (allowTurnInPlace) {
310      leftSpeed = xSpeed - zRotation;
311      rightSpeed = xSpeed + zRotation;
312    } else {
313      leftSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
314      rightSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
315    }
316
317    // Desaturate wheel speeds
318    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
319    if (maxMagnitude > 1.0) {
320      leftSpeed /= maxMagnitude;
321      rightSpeed /= maxMagnitude;
322    }
323
324    return new WheelSpeeds(leftSpeed, rightSpeed);
325  }
326
327  /**
328   * Tank drive inverse kinematics for differential drive platform.
329   *
330   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
331   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
332   *     positive.
333   * @param squareInputs If set, decreases the input sensitivity at low speeds.
334   * @return Wheel speeds [-1.0..1.0].
335   */
336  public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
337    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
338    rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
339
340    // Square the inputs (while preserving the sign) to increase fine control
341    // while permitting full power.
342    if (squareInputs) {
343      leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
344      rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
345    }
346
347    return new WheelSpeeds(leftSpeed, rightSpeed);
348  }
349
350  @Override
351  public void stopMotor() {
352    m_leftOutput = 0.0;
353    m_rightOutput = 0.0;
354
355    m_leftMotor.accept(0.0);
356    m_rightMotor.accept(0.0);
357
358    feed();
359  }
360
361  @Override
362  public String getDescription() {
363    return "DifferentialDrive";
364  }
365
366  @Override
367  public void initSendable(SendableBuilder builder) {
368    builder.setSmartDashboardType("DifferentialDrive");
369    builder.setActuator(true);
370    builder.setSafeState(this::stopMotor);
371    builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor);
372    builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor);
373  }
374}