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