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 java.util.Objects.requireNonNull;
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.SpeedController;
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. Motor outputs for the right side are negated, so motor direction inversion by the user is
075 * usually unnecessary.
076 *
077 * <p>This library uses the NED axes convention (North-East-Down as external reference in the world
078 * frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
079 *
080 * <p>The positive X axis points ahead, the positive Y axis points right, and the positive Z axis
081 * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
082 * positive.
083 *
084 * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
085 * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
086 * value can be changed with {@link #setDeadband}.
087 */
088@SuppressWarnings("removal")
089public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
090  private static int instances;
091
092  private final SpeedController m_leftMotor;
093  private final SpeedController m_rightMotor;
094
095  private boolean m_reported;
096
097  @SuppressWarnings("MemberName")
098  public static class WheelSpeeds {
099    public double left;
100    public double right;
101
102    /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
103    public WheelSpeeds() {}
104
105    /**
106     * Constructs a WheelSpeeds.
107     *
108     * @param left The left speed.
109     * @param right The right speed.
110     */
111    public WheelSpeeds(double left, double right) {
112      this.left = left;
113      this.right = right;
114    }
115  }
116
117  /**
118   * Construct a DifferentialDrive.
119   *
120   * <p>To pass multiple motors per side, use a {@link
121   * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}. If a motor needs to be inverted, do
122   * so before passing it in.
123   *
124   * @param leftMotor Left motor.
125   * @param rightMotor Right motor.
126   */
127  public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) {
128    requireNonNull(leftMotor, "Left motor cannot be null");
129    requireNonNull(rightMotor, "Right motor cannot be null");
130
131    m_leftMotor = leftMotor;
132    m_rightMotor = rightMotor;
133    SendableRegistry.addChild(this, m_leftMotor);
134    SendableRegistry.addChild(this, m_rightMotor);
135    instances++;
136    SendableRegistry.addLW(this, "DifferentialDrive", instances);
137  }
138
139  @Override
140  public void close() {
141    SendableRegistry.remove(this);
142  }
143
144  /**
145   * Arcade drive method for differential drive platform. The calculated values will be squared to
146   * decrease sensitivity at low speeds.
147   *
148   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
149   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
150   *     positive.
151   */
152  @SuppressWarnings("ParameterName")
153  public void arcadeDrive(double xSpeed, double zRotation) {
154    arcadeDrive(xSpeed, zRotation, true);
155  }
156
157  /**
158   * Arcade drive method for differential drive platform.
159   *
160   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
161   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
162   *     positive.
163   * @param squareInputs If set, decreases the input sensitivity at low speeds.
164   */
165  @SuppressWarnings("ParameterName")
166  public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
167    if (!m_reported) {
168      HAL.report(
169          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialArcade, 2);
170      m_reported = true;
171    }
172
173    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
174    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
175
176    var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
177
178    m_leftMotor.set(speeds.left * m_maxOutput);
179    m_rightMotor.set(speeds.right * m_maxOutput);
180
181    feed();
182  }
183
184  /**
185   * Curvature drive method for differential drive platform.
186   *
187   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
188   * heading change. This makes the robot more controllable at high speeds.
189   *
190   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
191   * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive.
192   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
193   *     maneuvers. zRotation will control turning rate instead of curvature.
194   */
195  @SuppressWarnings("ParameterName")
196  public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
197    if (!m_reported) {
198      HAL.report(
199          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialCurvature, 2);
200      m_reported = true;
201    }
202
203    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
204    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
205
206    var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
207
208    m_leftMotor.set(speeds.left * m_maxOutput);
209    m_rightMotor.set(speeds.right * m_maxOutput);
210
211    feed();
212  }
213
214  /**
215   * Tank drive method for differential drive platform. The calculated values will be squared to
216   * decrease sensitivity at low speeds.
217   *
218   * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is positive.
219   * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
220   *     positive.
221   */
222  public void tankDrive(double leftSpeed, double rightSpeed) {
223    tankDrive(leftSpeed, rightSpeed, true);
224  }
225
226  /**
227   * Tank drive method for differential drive platform.
228   *
229   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
230   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
231   *     positive.
232   * @param squareInputs If set, decreases the input sensitivity at low speeds.
233   */
234  public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
235    if (!m_reported) {
236      HAL.report(
237          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialTank, 2);
238      m_reported = true;
239    }
240
241    leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
242    rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
243
244    var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
245
246    m_leftMotor.set(speeds.left * m_maxOutput);
247    m_rightMotor.set(speeds.right * m_maxOutput);
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]. Clockwise is
257   *     positive.
258   * @param squareInputs If set, decreases the input sensitivity at low speeds.
259   * @return Wheel speeds.
260   */
261  @SuppressWarnings("ParameterName")
262  public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
263    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
264    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
265
266    // Square the inputs (while preserving the sign) to increase fine control
267    // while permitting full power.
268    if (squareInputs) {
269      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
270      zRotation = Math.copySign(zRotation * zRotation, zRotation);
271    }
272
273    double leftSpeed;
274    double rightSpeed;
275
276    double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
277
278    if (xSpeed >= 0.0) {
279      // First quadrant, else second quadrant
280      if (zRotation >= 0.0) {
281        leftSpeed = maxInput;
282        rightSpeed = xSpeed - zRotation;
283      } else {
284        leftSpeed = xSpeed + zRotation;
285        rightSpeed = maxInput;
286      }
287    } else {
288      // Third quadrant, else fourth quadrant
289      if (zRotation >= 0.0) {
290        leftSpeed = xSpeed + zRotation;
291        rightSpeed = maxInput;
292      } else {
293        leftSpeed = maxInput;
294        rightSpeed = xSpeed - zRotation;
295      }
296    }
297
298    // Normalize the wheel speeds
299    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
300    if (maxMagnitude > 1.0) {
301      leftSpeed /= maxMagnitude;
302      rightSpeed /= maxMagnitude;
303    }
304
305    return new WheelSpeeds(leftSpeed, rightSpeed);
306  }
307
308  /**
309   * Curvature drive inverse kinematics for differential drive platform.
310   *
311   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
312   * heading change. This makes the robot more controllable at high speeds.
313   *
314   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
315   * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is positive.
316   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
317   *     maneuvers. zRotation will control rotation rate around the Z axis instead of curvature.
318   * @return Wheel speeds.
319   */
320  @SuppressWarnings("ParameterName")
321  public static WheelSpeeds curvatureDriveIK(
322      double xSpeed, double zRotation, boolean allowTurnInPlace) {
323    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
324    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
325
326    double leftSpeed;
327    double rightSpeed;
328
329    if (allowTurnInPlace) {
330      leftSpeed = xSpeed + zRotation;
331      rightSpeed = xSpeed - zRotation;
332    } else {
333      leftSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
334      rightSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
335    }
336
337    // Normalize wheel speeds
338    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
339    if (maxMagnitude > 1.0) {
340      leftSpeed /= maxMagnitude;
341      rightSpeed /= maxMagnitude;
342    }
343
344    return new WheelSpeeds(leftSpeed, rightSpeed);
345  }
346
347  /**
348   * Tank drive inverse kinematics for differential drive platform.
349   *
350   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
351   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
352   *     positive.
353   * @param squareInputs If set, decreases the input sensitivity at low speeds.
354   * @return Wheel speeds.
355   */
356  public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
357    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
358    rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
359
360    // Square the inputs (while preserving the sign) to increase fine control
361    // while permitting full power.
362    if (squareInputs) {
363      leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
364      rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
365    }
366
367    return new WheelSpeeds(leftSpeed, rightSpeed);
368  }
369
370  @Override
371  public void stopMotor() {
372    m_leftMotor.stopMotor();
373    m_rightMotor.stopMotor();
374    feed();
375  }
376
377  @Override
378  public String getDescription() {
379    return "DifferentialDrive";
380  }
381
382  @Override
383  public void initSendable(SendableBuilder builder) {
384    builder.setSmartDashboardType("DifferentialDrive");
385    builder.setActuator(true);
386    builder.setSafeState(this::stopMotor);
387    builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
388    builder.addDoubleProperty(
389        "Right Motor Speed", () -> m_rightMotor.get(), x -> m_rightMotor.set(x));
390  }
391}